Merge pull request fkanehiro#921 from snozawa/update_graspless

Update graspless manip mode

rtc/AutoBalancer/AutoBalancer.cpp

@@ -838,7 +838,7 @@ void AutoBalancer::getTargetParameters()
  }
  //
  {
- if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
+ if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
  hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
  gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
  }// else {
@@ -1800,14 +1800,16 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_
  ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
  hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
  if ( graspless_manip_arm == "arms" ) {
- // act_hand_coords.pos = (target_coords["rarm"].pos + target_coords["larm"].pos) / 2.0;
- // vector3 cur_y(target_coords["larm"].pos - target_coords["rarm"].pos);
- // cur_y(2) = 0;
- // alias(cur_y) = normalize(cur_y);
- // vector3 ref_y(ref_hand_coords.axis(AXIS_Y));
- // ref_y(2) = 0;
- // alias(ref_y) = normalize(ref_y);
- // dr = 0,0,((vector3(cross(ref_y, cur_y))(2) > 0 ? 1.0 : -1.0) * std::acos(dot(ref_y, cur_y))); // fix for rotation
+ hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+ hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
+ act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+ hrp::Vector3 act_y = larm_pos-rarm_pos;
+ act_y(2) = 0;
+ act_y.normalize();
+ hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+ ref_y(2) = 0;
+ ref_y.normalize();
+ dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
  } else {
  ABCIKparam& tmpikp = ikp[graspless_manip_arm];
  act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,

sample/SampleRobot/samplerobot_auto_balancer.py

@@ -70,16 +70,25 @@ def Quaternion2RotMatrixZ(q):
  [numpy.sin(theta), numpy.cos(theta), 0],
  [ 0, 0, 1]])
 
-def checkGoPosParam (goalx, goaly, goalth, prev_dst_foot_midcoords):
- '''Check whether goPos argument are correctly achieved based on dst_foot_midcoords values.
- goPos params should be "new_dst_foot_midcoords - prev_dst_foot_midcoords"
+
+def calcDiffFootMidCoords (prev_dst_foot_midcoords):
+ '''Calculate difference from previous dst_foot_midcoords and current dst_foot_midcoords.
+ Returns difx, dify, difth, which are gopos parameters
  '''
  new_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
  # Check diff
  difxy = (Quaternion2RotMatrixZ(prev_dst_foot_midcoords.rot).transpose()).dot((numpy.array([new_dst_foot_midcoords.pos])-numpy.array([prev_dst_foot_midcoords.pos])).transpose())
  difth = math.degrees(Quaternion2Angle(new_dst_foot_midcoords.rot)-Quaternion2Angle(prev_dst_foot_midcoords.rot))
- ret = (abs(difxy[0,0]-goalx) < 1e-5 and abs(difxy[1,0]-goaly) < 1e-5 and abs(difth-goalth) < 1e-5)
- print >> sys.stderr, " Check goPosParam (diff = ", (difxy[0,0]-goalx), "[m], ", (difxy[1,0]-goaly), "[m], ", (difth-goalth), "[deg])"
+ return [difxy[0,0], difxy[1,0], difth]
+
+def checkGoPosParam (goalx, goaly, goalth, prev_dst_foot_midcoords):
+ '''Check whether goPos argument are correctly achieved based on dst_foot_midcoords values.
+ goPos params should be "new_dst_foot_midcoords - prev_dst_foot_midcoords"
+ '''
+ # Check diff
+ [difx, dify, difth] = calcDiffFootMidCoords(prev_dst_foot_midcoords)
+ ret = (abs(difx-goalx) < 1e-5 and abs(dify-goaly) < 1e-5 and abs(difth-goalth) < 1e-5)
+ print >> sys.stderr, " Check goPosParam (diff = ", (difx-goalx), "[m], ", (dify-goaly), "[m], ", (difth-goalth), "[deg])"
  print >> sys.stderr, " => ", ret
  assert(ret)
  return ret
@@ -439,7 +448,7 @@ def demoGaitGeneratorFixHand():
  abcp.default_zmp_offsets=[[0.01, 0.0, 0.0], [0.01, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # Setting default_zmp_offsets is not necessary for fix mode. Just for debugging for default_zmp_offsets in hand fix mode.
  hcf.abc_svc.setAutoBalancerParam(abcp)
  dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163]
- hcf.seq_svc.setJointAngles(dualarm_push_pose, 2.0)
+ hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
  hcf.waitInterpolation()
  print >> sys.stderr, " Walk without fixing arm" 
  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
@@ -457,9 +466,9 @@ def demoGaitGeneratorFixHand():
  hcf.abc_svc.setAutoBalancerParam(abcp)
  hcf.abc_svc.goPos(0.3,0,0)
  hcf.abc_svc.waitFootSteps()
- hcf.abc_svc.goPos(0,0.2,0)
+ hcf.abc_svc.goPos(0,-0.2,0)
  hcf.abc_svc.waitFootSteps()
- hcf.abc_svc.goPos(0,0,30)
+ hcf.abc_svc.goPos(0,0,-30)
  hcf.abc_svc.waitFootSteps()
  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
  abcp.is_hand_fix_mode=False
@@ -518,8 +527,71 @@ def demoGaitGeneratorGoPosOverwrite():
  hcf.abc_svc.waitFootSteps()
  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
 
+def demoGaitGeneratorGrasplessManipMode():
+ print >> sys.stderr, "17. Graspless manip mode"
+ hcf.startAutoBalancer();
+ # Initialize and pose define
+ dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163]
+ hcf.seq_svc.setJointAngles(dualarm_push_pose, 0.5)
+ hcf.waitInterpolation()
+ # hands 50[mm] fwd from dualarm_push_pose
+ av_fwd = [-5.579249e-05,-0.760285,-0.000277,1.44619,-0.660772,-2.615057e-05,-0.7752,-0.080815,0.116555,-0.935667,-1.70514,-0.045373,1.309,-5.577374e-05,-0.760232,-0.000277,1.44608,-0.660715,-2.613350e-05,-0.77525,0.080663,-0.116463,-0.935597,1.70494,-0.045325,-1.309,0.157668,0.000123,-0.000152]
+ # hands 50[mm] bwd from dualarm_push_pose
+ av_bwd = [-1.901820e-05,-0.641174,-0.00025,1.36927,-0.717047,-2.260319e-05,-0.305537,-0.099557,0.134675,-1.04208,-1.72497,-0.065256,1.309,-1.900236e-05,-0.641122,-0.00025,1.36915,-0.71698,-2.258509e-05,-0.305624,0.099383,-0.134605,-1.04197,1.72476,-0.06517,-1.309,-0.22394,5.625198e-05,-0.000165]
+ # hands 50[mm] right from dualarm_push_pose
+ av_right = [-0.005678,-0.711398,0.006148,1.40852,-0.678974,0.011103,-0.575284,-0.179786,0.092155,-0.999366,-1.74805,0.048205,1.309,-0.005686,-0.710309,0.006143,1.4098,-0.681345,0.011103,-0.520691,0.002033,-0.154878,-1.05585,1.6731,-0.161177,-1.309,0.015053,0.024788,-0.023196]
+ # hands 50[mm] left from dualarm_push_pose
+ av_left = [0.005607,-0.71036,-0.006671,1.40991,-0.681404,-0.011151,-0.52064,-0.002193,0.154967,-1.05593,-1.67329,-0.161246,1.309,0.005598,-0.711343,-0.006677,1.4084,-0.67891,-0.011151,-0.575342,0.179622,-0.092066,-0.999287,1.74785,0.04827,-1.309,0.015056,-0.024583,0.02287]
+ # hands 10[deg] right turn from dualarm_push_pose
+ av_rturn = [0.023512,-0.71245,0.014216,1.40899,-0.677868,-0.01575,-0.462682,0.040789,0.154221,-1.10667,-1.66067,-0.2349,1.309,0.023442,-0.708029,0.014161,1.40823,-0.68153,-0.015763,-0.61987,0.217174,-0.105089,-0.949927,1.75163,0.120793,-1.309,0.013747,-0.058774,-0.084435]
+ # hands 10[deg] left turn from dualarm_push_pose
+ av_lturn = [-0.023522,-0.708079,-0.014689,1.40835,-0.681597,0.015717,-0.619803,-0.217337,0.105179,-0.950004,-1.75184,0.120733,1.309,-0.02359,-0.712393,-0.014744,1.40889,-0.677813,0.0157,-0.462722,-0.040951,-0.154135,-1.10659,1.66048,-0.234826,-1.309,0.013715,0.058979,0.084109]
+ # parameter setting
+ org_abcp=hcf.abc_svc.getAutoBalancerParam()[1]
+ abcp=hcf.abc_svc.getAutoBalancerParam()[1]
+ abcp.graspless_manip_mode=True
+ abcp.is_hand_fix_mode=True
+ abcp.graspless_manip_reference_trans_rot=[1.0, 0.0, 0.0, 1.365307e-06] # trans rot for dualarm_push_pose
+ abcp.graspless_manip_reference_trans_pos=[0.450037, 1.049436e-06, 0.869818] # trans pos for dualarm_push_pose
+ abcp.graspless_manip_p_gain=[1,1,1]
+ hcf.abc_svc.setAutoBalancerParam(abcp)
+ hcf.co_svc.disableCollisionDetection()
+ # Check one foot_midcoords movement
+ gv_pose_list = [av_fwd, av_bwd, av_left, av_right, av_lturn, av_rturn]
+ ref_footmid_diff = [[50*1e-3,0,0],
+ [-50*1e-3,0,0],
+ [0, 0.5*50*1e-3,0], # 0.5->inside limitation
+ [0,-0.5*50*1e-3,0], # 0.5->inside limitation
+ [0,0, 0.5*10], # 0.5->inside limitation
+ [0,0,-0.5*10]] # 0.5->inside limitation
+ ret=True
+ hcf.abc_svc.waitFootSteps()
+ for idx in range(len(gv_pose_list)):
+ pose = gv_pose_list[idx]
+ prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
+ hcf.abc_svc.goVelocity(0,0,0);
+ hcf.seq_svc.setJointAngles(pose, 0.4)
+ hcf.waitInterpolation()
+ hcf.seq_svc.setJointAngles(pose, 1.6);hcf.waitInterpolation() # Dummy 2step
+ hcf.abc_svc.goStop()
+ diff=numpy.array(ref_footmid_diff[idx])-numpy.array(calcDiffFootMidCoords(prev_dst_foot_midcoords))
+ if idx == 4 or idx == 5:
+ tmpret = abs(diff[2]) < 1.0 # TODO, check pos
+ else:
+ tmpret = abs(diff[0]) < 1e-3 and abs(diff[1]) < 1e-3 and abs(diff[2]) < 1.0
+ ret = ret and tmpret
+ print >> sys.stderr, " ret = ", tmpret, ", diff = ", diff
+ # Finishing
+ if ret:
+ print >> sys.stderr, " total is OK"
+ assert(ret)
+ hcf.co_svc.enableCollisionDetection()
+ hcf.seq_svc.setJointAngles(dualarm_push_pose, 0.5)
+ hcf.waitInterpolation()
+ hcf.abc_svc.setAutoBalancerParam(org_abcp)
+
 def demoGaitGeneratorSetFootStepsWithArms():
- print >> sys.stderr, "17. Trot Walking"
+ print >> sys.stderr, "18. Trot Walking"
  hcf.stopAutoBalancer()
  hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0)
  hcf.waitInterpolation()
@@ -549,6 +621,13 @@ def demoGaitGeneratorSetFootStepsWithArms():
  hcf.abc_svc.setGaitGeneratorParam(orig_ggp)
  hcf.startAutoBalancer()
 
+def demoStandingPosResetting():
+ print >> sys.stderr, "demoStandingPosResetting"
+ hcf.abc_svc.goPos(0,0,math.degrees(-1*rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y));
+ hcf.abc_svc.waitFootSteps()
+ hcf.abc_svc.goPos(0,-1*rtm.readDataPort(hcf.rh.port("WAIST")).data.position.y,0);
+ hcf.abc_svc.waitFootSteps()
+
 def demo():
  init()
  if hrpsys_version >= '315.5.0':
@@ -576,9 +655,11 @@ def demo():
  demoGaitGeneratorChangeStepParam()
  demoGaitGeneratorOverwriteFootsteps()
  demoGaitGeneratorOverwriteFootsteps(2)
+ demoStandingPosResetting()
  demoGaitGeneratorFixHand()
  demoGaitGeneratorOverwriteCurrentFootstep()
  demoGaitGeneratorGoPosOverwrite()
+ demoGaitGeneratorGrasplessManipMode()
  demoGaitGeneratorSetFootStepsWithArms()
 
 if __name__ == '__main__':