sample4legrobot_stabilizer.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 import math
00017 
00018 def init ():
00019     global hcf, initial_pose, hrpsys_version
00020     hcf = HrpsysConfigurator()
00021     hcf.getRTCList = hcf.getRTCListUnstable
00022     hcf.init ("Sample4LegRobot(Robot)0", "$(PROJECT_DIR)/../model/sample_4leg_robot.wrl")
00023     initial_pose = [0,  -0.378613,  0,  0.832038,  -0.452564,  0,
00024                     0,  -0.378613,  0,  0.832038,  -0.452564,  0,
00025                     0,  -0.378613,  0,  0.832038,  -0.452564,  0,
00026                     0,  -0.378613,  0,  0.832038,  -0.452564,  0,]
00027     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00028     hcf.waitInterpolation()
00029     # decrease zmp weight for arms
00030     abcp=hcf.abc_svc.getAutoBalancerParam()[1]
00031     abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
00032     hcf.abc_svc.setAutoBalancerParam(abcp)
00033     ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00034     ggp.zmp_weight_map = [1.0]*4
00035     ggp.default_step_height = 0.05
00036     hcf.abc_svc.setGaitGeneratorParam(ggp)
00037     hcf.startAutoBalancer(['rleg', 'lleg', 'rarm', 'larm'])
00038     hrpsys_version = hcf.seq.ref.get_component_profile().version
00039     print("hrpsys_version = %s"%hrpsys_version)
00040 
00041 def demoSetParameterAndStartST():
00042     print >> sys.stderr, "1. setParameter"
00043     stp_org = hcf.st_svc.getParameter()
00044     # for tpcc
00045     stp_org.k_tpcc_p=[0.2, 0.2]
00046     stp_org.k_tpcc_x=[4.0, 4.0]
00047     stp_org.k_brot_p=[0.0, 0.0]
00048     # for eefm
00049     tmp_leg_inside_margin=71.12*1e-3
00050     tmp_leg_outside_margin=71.12*1e-3
00051     tmp_leg_front_margin=182.0*1e-3
00052     tmp_leg_rear_margin=72.0*1e-3
00053     rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
00054                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
00055                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
00056                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
00057     lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
00058                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
00059                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
00060                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
00061     rarm_vertices = rleg_vertices
00062     larm_vertices = lleg_vertices
00063     stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
00064     stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin
00065     stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin
00066     stp_org.eefm_leg_front_margin=tmp_leg_front_margin
00067     stp_org.eefm_leg_rear_margin=tmp_leg_rear_margin
00068     stp_org.eefm_k1=[-1.39899,-1.39899]
00069     stp_org.eefm_k2=[-0.386111,-0.386111]
00070     stp_org.eefm_k3=[-0.175068,-0.175068]
00071     stp_org.eefm_rot_damping_gain = [[20*1.6*1.5, 20*1.6*1.5, 1e5]]*4
00072     stp_org.eefm_pos_damping_gain = [[3500*50, 3500*50, 3500*1.0*1.5]]*4
00073     stp_org.is_ik_enable = [True]*4
00074     stp_org.is_feedback_control_enable = [True]*4
00075     stp_org.is_zmp_calc_enable = [True]*4
00076     stp_org.eefm_use_force_difference_control=False
00077     stp_org.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP
00078     hcf.st_svc.setParameter(stp_org)
00079     hcf.startStabilizer ()
00080 
00081 def demoSetFootStepsWithST():
00082     print >> sys.stderr,"2. setFootSteps"
00083     hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg"),
00084                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.00,+0.19,0], [1,0,0,0], "larm")]),
00085                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.15,+0.19,0], [1,0,0,0], "lleg"),
00086                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.15,-0.19,0], [1,0,0,0], "rarm")]),
00087                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.30,-0.19,0], [1,0,0,0], "rleg"),
00088                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.30,+0.19,0], [1,0,0,0], "larm")]),
00089                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.45,+0.19,0], [1,0,0,0], "lleg"),
00090                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.45,-0.19,0], [1,0,0,0], "rarm")]),
00091                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.45,-0.19,0], [1,0,0,0], "rleg"),
00092                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.45,+0.19,0], [1,0,0,0], "larm")])])
00093     hcf.abc_svc.waitFootSteps()
00094 
00095 def demo():
00096     init()
00097     demoSetParameterAndStartST()
00098     demoSetFootStepsWithST()
00099 
00100 if __name__ == '__main__':
00101     demo()


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