00001
00002
00003 try:
00004 from hrpsys.hrpsys_config import *
00005 import OpenHRP
00006 except:
00007 print "import without hrpsys"
00008 import rtm
00009 from rtm import *
00010 from OpenHRP import *
00011 import waitInput
00012 from waitInput import *
00013 import socket
00014 import time
00015
00016 from subprocess import check_output
00017
00018
00019 def demoSetParameter():
00020 stp_org = hcf.st_svc.getParameter()
00021
00022 stp_org.k_tpcc_p=[0.2, 0.2]
00023 stp_org.k_tpcc_x=[4.0, 4.0]
00024 stp_org.k_brot_p=[0.0, 0.0]
00025
00026 tmp_leg_inside_margin=71.12*1e-3
00027 tmp_leg_outside_margin=71.12*1e-3
00028 tmp_leg_front_margin=182.0*1e-3
00029 tmp_leg_rear_margin=72.0*1e-3
00030 rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
00031 OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
00032 OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
00033 OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
00034 lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
00035 OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
00036 OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
00037 OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
00038 rarm_vertices = rleg_vertices
00039 larm_vertices = lleg_vertices
00040 stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
00041 stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin
00042 stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin
00043 stp_org.eefm_leg_front_margin=tmp_leg_front_margin
00044 stp_org.eefm_leg_rear_margin=tmp_leg_rear_margin
00045 stp_org.eefm_k1=[-1.39899,-1.39899]
00046 stp_org.eefm_k2=[-0.386111,-0.386111]
00047 stp_org.eefm_k3=[-0.175068,-0.175068]
00048 stp_org.eefm_rot_damping_gain = [[20*1.6*1.5, 20*1.6*1.5, 1e5]]*4
00049 stp_org.eefm_pos_damping_gain = [[3500*50, 3500*50, 3500*1.0*1.5]]*4
00050 stp_org.st_algorithm=OpenHRP.StabilizerService.EEFMQP
00051 stp_org.is_ik_enable=[True]*4
00052 hcf.st_svc.setParameter(stp_org)
00053
00054 def defJointGroups ():
00055 rleg_6dof_group = ['rleg', ['RLEG_HIP_R', 'RLEG_HIP_P', 'RLEG_HIP_Y', 'RLEG_KNEE', 'RLEG_ANKLE_P', 'RLEG_ANKLE_R']]
00056 lleg_6dof_group = ['lleg', ['LLEG_HIP_R', 'LLEG_HIP_P', 'LLEG_HIP_Y', 'LLEG_KNEE', 'LLEG_ANKLE_P', 'LLEG_ANKLE_R']]
00057 torso_group = ['torso', ['WAIST_P', 'WAIST_R', 'CHEST']]
00058 head_group = ['head', []]
00059 rarm_group = ['rarm', ['RARM_SHOULDER_P', 'RARM_SHOULDER_R', 'RARM_SHOULDER_Y', 'RARM_ELBOW', 'RARM_WRIST_Y', 'RARM_WRIST_P', 'RARM_WRIST_R']]
00060 larm_group = ['larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']]
00061 return [rleg_6dof_group, lleg_6dof_group, torso_group, head_group, rarm_group, larm_group]
00062
00063 def init ():
00064 global hcf, initial_pose, dualarm_via_pose, dualarm_reach_pose, dualarm_liftup_pose, singlearm_via_pose, singlearm_reach_pose, singlearm_liftup_pose, dualarm_push_pose
00065 hcf = HrpsysConfigurator()
00066 hcf.getRTCList = hcf.getRTCListUnstable
00067 hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00068
00069 initial_pose=[-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,0.993363,-0.165936,-0.232906,-1.64155,-0.009413,-0.362925,0.0,-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,0.993407,0.16582,0.232707,-1.64166,0.009397,-0.362966,0.0,0.0,0.0,0.0]
00070 dualarm_via_pose=[-1.301383e-16,-0.4932,-4.784822e-17,1.26321,-0.737682,1.436666e-16,0.584706,-0.143933,0.086868,-1.76694,-1.69575,-0.112369,0.637045,-2.103971e-16,-0.4932,-3.815624e-17,1.26321,-0.737682,2.133439e-16,0.584706,0.143933,-0.086868,-1.76694,1.69575,-0.112369,-0.637045,-0.369579,7.202322e-16,1.814631e-16]
00071 dualarm_reach_pose=[1.552547e-16,-0.531004,7.070416e-17,1.21764,-0.67113,-9.670336e-17,-0.049061,-0.07576,0.074177,-1.18878,-1.66902,-0.040133,0.637045,5.975712e-17,-0.531004,6.854620e-17,1.21764,-0.67113,-1.725813e-17,-0.049061,0.07576,-0.074177,-1.18878,1.66902,-0.040133,-0.637045,-0.322025,-1.568538e-16,-4.382552e-16]
00072 dualarm_liftup_pose=[4.954412e-15,-0.524931,8.930202e-15,1.07007,-0.525833,-3.445727e-16,-0.16089,-0.406321,0.424654,-1.57558,-2.01019,-0.390167,0.637045,4.790551e-15,-0.524931,8.955748e-15,1.07007,-0.525833,-1.750785e-16,-0.16089,0.406321,-0.424654,-1.57558,2.01019,-0.390167,-0.637045,0.007999,4.191514e-16,-4.457395e-14]
00073 singlearm_via_pose = [-0.072674,-0.322265,0.02671,0.793691,-0.449511,0.091832,0.930492,-0.172216,0.271035,-2.15462,-0.94786,-0.274689,0.637045,-0.072879,-0.32137,0.026706,0.807374,-0.464093,0.092013,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,-0.210667,0.569051,-0.203392]
00074 singlearm_reach_pose = [-0.065582,-0.348364,-0.036375,0.770359,-0.384613,0.05823,0.267732,0.067753,-0.2238,-1.88388,-1.20653,-0.157041,0.637045,-0.066042,-0.362442,-0.036569,0.777637,-0.377806,0.05814,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.119774,0.357853,0.377041]
00075 singlearm_liftup_pose = [-0.043429,-0.330743,-0.038834,0.732611,-0.37014,0.028879,0.256358,0.163926,-0.247853,-1.90506,-1.28971,-0.110907,0.637045,-0.043613,-0.34042,-0.038972,0.729501,-0.357353,0.028662,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.089304,0.185283,0.369292]
00076 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]
00077 hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00078 hcf.waitInterpolation()
00079
00080 hcf.co_svc.disableCollisionDetection()
00081 ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
00082 ggp.stride_parameter=[0.15, 0.08, 25.0, 0.15];
00083 hcf.abc_svc.setGaitGeneratorParam(ggp);
00084 demoSetParameter()
00085 icp=hcf.ic_svc.getImpedanceControllerParam("rarm")[1]
00086
00087 icp.D_p = 400
00088 icp.K_r = 1e5
00089 icp.D_r = 1e5
00090 hcf.ic_svc.setImpedanceControllerParam("rarm", icp)
00091 hcf.ic_svc.setImpedanceControllerParam("larm", icp)
00092 hcf.Groups = defJointGroups()
00093 hcf.startDefaultUnstableControllers(['rarm', 'larm'], ["rleg", "lleg", "rarm", "larm"])
00094 HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip()
00095 hcf.rmfo_svc.loadForceMomentOffsetParams(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/ForceSensorOffset_SampleRobot.txt')
00096
00097 def demoDualarmCarryup (is_walk=True, auto_detecion = True):
00098 print >> sys.stderr, "1. Dual-arm carry-up demo."
00099 print >> sys.stderr, " Reaching"
00100 hcf.seq_svc.setJointAngles(dualarm_via_pose, 2.0)
00101 hcf.waitInterpolation()
00102 hcf.seq_svc.setJointAngles(dualarm_reach_pose, 2.0)
00103 hcf.waitInterpolation()
00104 print >> sys.stderr, " Increase operational force"
00105 if auto_detecion:
00106 objectTurnaroundDetection()
00107 else:
00108 hcf.seq_svc.setWrenches([0]*6+[0]*6+[0,0,-9.8*2.5,0,0,0]*2, 2.0)
00109 hcf.waitInterpolation()
00110 print >> sys.stderr, " Lift up & down"
00111 hcf.seq_svc.setJointAngles(dualarm_liftup_pose, 2.0)
00112 hcf.waitInterpolation()
00113 if is_walk:
00114 demoWalk()
00115 hcf.seq_svc.setJointAngles(dualarm_reach_pose, 2.0)
00116 hcf.waitInterpolation()
00117 print >> sys.stderr, " Reset operational force"
00118 hcf.seq_svc.setWrenches([0]*24, 2.0)
00119 hcf.waitInterpolation()
00120 print >> sys.stderr, " Releasing"
00121 hcf.seq_svc.setJointAngles(dualarm_via_pose, 2.0)
00122 hcf.waitInterpolation()
00123 hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00124 hcf.waitInterpolation()
00125
00126 def demoSinglearmCarryup (is_walk=True, auto_detecion = True):
00127 print >> sys.stderr, "2. Single-arm carry-up demo."
00128 print >> sys.stderr, " Reaching"
00129 hcf.abc_svc.goPos(0.02,0.15,0)
00130 hcf.abc_svc.waitFootSteps();
00131 hcf.seq_svc.setJointAngles(singlearm_via_pose, 2.0)
00132 hcf.waitInterpolation()
00133 hcf.seq_svc.setJointAngles(singlearm_reach_pose, 2.0)
00134 hcf.waitInterpolation()
00135 print >> sys.stderr, " Increase operational force"
00136 if auto_detecion:
00137 objectTurnaroundDetection(limbs=['rarm'])
00138 else:
00139 hcf.seq_svc.setWrenches([0]*6+[0]*6+[0]*6+[0,0,-9.8*5.0,0,0,0], 2.0)
00140 hcf.waitInterpolation()
00141 print >> sys.stderr, " Lift up & down"
00142 hcf.seq_svc.setJointAngles(singlearm_liftup_pose, 2.0)
00143 hcf.waitInterpolation()
00144 if is_walk:
00145 hcf.setJointAngle("RARM_WRIST_R", 10, 0.3)
00146 hcf.waitInterpolation()
00147 demoWalk()
00148 hcf.seq_svc.setJointAngles(singlearm_reach_pose, 2.0)
00149 hcf.waitInterpolation()
00150 print >> sys.stderr, " Reset operational force"
00151 hcf.seq_svc.setWrenches([0]*24, 2.0)
00152 hcf.waitInterpolation()
00153 print >> sys.stderr, " Releasing"
00154 hcf.seq_svc.setJointAngles(singlearm_via_pose, 2.0)
00155 hcf.waitInterpolation()
00156 hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00157 hcf.waitInterpolation()
00158
00159 def objectTurnaroundDetection(max_time = 4.0, max_ref_force = 9.8*6.0, limbs=["rarm", "larm"], axis=[0,0,-1]):
00160 octdp=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1]
00161 octdp.detect_time_thre=0.3
00162 octdp.start_time_thre=0.3
00163 octdp.axis=axis
00164 hcf.octd_svc.setObjectContactTurnaroundDetectorParam(octdp)
00165 if limbs==['rarm']:
00166 force = [axis[0]*max_ref_force, axis[1]*max_ref_force, axis[2]*max_ref_force]
00167 hcf.seq_svc.setWrenches([0]*18+force+[0,0,0], max_time)
00168 else:
00169 force = [axis[0]*max_ref_force*0.5, axis[1]*max_ref_force*0.5, axis[2]*max_ref_force*0.5]
00170 hcf.seq_svc.setWrenches([0]*12+force+[0]*3+force+[0]*3, max_time)
00171 hcf.octd_svc.startObjectContactTurnaroundDetection(max_ref_force, max_time+2.0, limbs)
00172 flg = True
00173 while flg:
00174 tmpflg = hcf.octd_svc.checkObjectContactTurnaroundDetection()
00175
00176 [ret, fv, mv, total, fric_w] = hcf.octd_svc.getObjectForcesMoments()
00177 print " flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w
00178 flg = (tmpflg == OpenHRP.ObjectContactTurnaroundDetectorService.MODE_DETECTOR_IDLE) or (tmpflg == OpenHRP.ObjectContactTurnaroundDetectorService.MODE_STARTED)
00179 time.sleep(0.5)
00180 [ret, fv, mv, total, fric_w] = hcf.octd_svc.getObjectForcesMoments()
00181 print " flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w
00182 if limbs==['rarm']:
00183 hcf.seq_svc.setWrenches([0]*18+fv[0]+mv[0], 2.0)
00184 else:
00185 hcf.seq_svc.setWrenches([0]*12+fv[1]+mv[1]+fv[0]+mv[0], 2.0)
00186 hcf.waitInterpolation()
00187
00188 def demoWalk ():
00189 hcf.abc_svc.goPos(-0.3,-0.1,0);
00190 hcf.abc_svc.waitFootSteps();
00191 hcf.abc_svc.goPos(0,0,30);
00192 hcf.abc_svc.waitFootSteps();
00193 hcf.abc_svc.goPos(0,0,-30);
00194 hcf.abc_svc.waitFootSteps();
00195 hcf.abc_svc.goPos(0.3,0.1,0);
00196 hcf.abc_svc.waitFootSteps();
00197
00198 def demoDualarmPush (auto_detecion = True):
00199 print >> sys.stderr, "3. Dual-arm push demo."
00200 print >> sys.stderr, " Move to"
00201 hcf.abc_svc.goPos(-0.45,0,0);
00202 hcf.abc_svc.waitFootSteps();
00203 hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
00204 hcf.abc_svc.waitFootSteps();
00205 print >> sys.stderr, " Reaching"
00206
00207 hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
00208 hcf.abc_svc.waitFootSteps();
00209 hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
00210 hcf.waitInterpolation()
00211 hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
00212 hcf.waitInterpolation()
00213 print >> sys.stderr, " Increase operational force"
00214 if auto_detecion:
00215 objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
00216 else:
00217 hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
00218 hcf.waitInterpolation()
00219 print >> sys.stderr, " Push forward"
00220 abcp=hcf.abc_svc.getAutoBalancerParam()[1]
00221 abcp.is_hand_fix_mode = True
00222 hcf.abc_svc.setAutoBalancerParam(abcp)
00223 hcf.abc_svc.goPos(0.5,0,0)
00224 hcf.abc_svc.waitFootSteps();
00225 hcf.seq_svc.setWrenches([0]*24, 2.0)
00226 hcf.waitInterpolation()
00227
00228 def demo ():
00229 init()
00230 demoDualarmCarryup()
00231 demoSinglearmCarryup()
00232 demoDualarmPush()
00233
00234 if __name__ == '__main__':
00235 demo()