samplerobot_carry_object.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 # set parameter
00019 def demoSetParameter():
00020     stp_org = hcf.st_svc.getParameter()
00021     # for tpcc
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     # for eefm
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     # Pose setting
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     # Initialize controllers
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     #icp.K_p = 400
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) # 2.5[kg]*2 = 5.0[kg]
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         #print rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data, rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data
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     #hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
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()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56