hrpsys: sample4legrobot_auto_balancer.py Source File

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.000135, -0.523291, -0.000225, 1.15585, -0.631694, 0.000267,
00024                     -0.000135, -0.523291, -0.000225, 1.15585, -0.631694, 0.000267,
00025                     -0.000113, -0.523291, -0.000225, 1.15585, -0.631693, 0.000245,
00026                     -0.000113, -0.523291, -0.000225, 1.15585, -0.631693, 0.000245]
00027     # initial_pose = [0,  -0.378613,  0,  0.832038,  -0.452564,  0,
00028     #                 0,  -0.378613,  0,  0.832038,  -0.452564,  0,
00029     #                 0,  -0.378613,  0,  0.832038,  -0.452564,  0,
00030     #                 0,  -0.378613,  0,  0.832038,  -0.452564,  0,]
00031     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00032     hcf.waitInterpolation()
00033     # set abc param
00034     abcp=hcf.abc_svc.getAutoBalancerParam()[1]
00035     abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
00036     hcf.abc_svc.setAutoBalancerParam(abcp)
00037     # set gg param
00038     ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00039     ggp.leg_default_translate_pos = [[0.0, -0.19, 0.0], [0.0, 0.19, 0.0], [0.7, -0.19, 0.0], [0.7, 0.19, 0.0]]
00040     ggp.zmp_weight_map = [1.0]*4
00041     ggp.default_step_height = 0.065 # see https://github.com/fkanehiro/hrpsys-base/issues/801
00042     hcf.abc_svc.setGaitGeneratorParam(ggp)
00043     hcf.startAutoBalancer(limbs=['rleg','lleg','rarm','larm'])
00044     hrpsys_version = hcf.seq.ref.get_component_profile().version
00045     print("hrpsys_version = %s"%hrpsys_version)
00046 
00047 def demoGaitGeneratorSetFootSteps(print_str="1. setFootSteps"):
00048     print >> sys.stderr, print_str
00049     hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg"),
00050                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.00,+0.19,0], [1,0,0,0], "larm")]),
00051                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.15,+0.19,0], [1,0,0,0], "lleg"),
00052                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.15,-0.19,0], [1,0,0,0], "rarm")]),
00053                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.30,-0.19,0], [1,0,0,0], "rleg"),
00054                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.30,+0.19,0], [1,0,0,0], "larm")]),
00055                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.45,+0.19,0], [1,0,0,0], "lleg"),
00056                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.45,-0.19,0], [1,0,0,0], "rarm")]),
00057                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.45,-0.19,0], [1,0,0,0], "rleg"),
00058                                                              OpenHRP.AutoBalancerService.Footstep([0.7+0.45,+0.19,0], [1,0,0,0], "larm")])])
00059     hcf.abc_svc.waitFootSteps()
00060 
00061 def demoGaitGeneratorSetFootStepsRectangle():
00062     ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
00063     ggp.swing_trajectory_delay_time_offset=0.05;
00064     ggp.default_orbit_type=OpenHRP.AutoBalancerService.RECTANGLE;
00065     hcf.abc_svc.setGaitGeneratorParam(ggp)
00066     demoGaitGeneratorSetFootSteps("2. setFootSteps with Rectangle orbit");
00067 
00068 def demoGaitGeneratorSetFootStepsCycloidDelay():
00069     ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
00070     ggp.swing_trajectory_delay_time_offset=0.05;
00071     ggp.swing_trajectory_final_distance_weight=3.0;
00072     ggp.default_orbit_type=OpenHRP.AutoBalancerService.CYCLOIDDELAY;
00073     hcf.abc_svc.setGaitGeneratorParam(ggp)
00074     demoGaitGeneratorSetFootSteps("3. setFootSteps with Cycloiddelay orbit");
00075 
00076 def demoGaitGeneratorSetFootStepsCrawl(print_str="4. setFootSteps in Crawl"):
00077     print >> sys.stderr, print_str
00078     hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg")]),
00079                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.7+0.01,+0.19,0], [1,0,0,0], "larm")]),
00080                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.01,+0.19,0], [1,0,0,0], "lleg")]),
00081                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.7+0.01,-0.19,0], [1,0,0,0], "rarm")]),
00082                       OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.01,-0.19,0], [1,0,0,0], "rleg")])])
00083     hcf.abc_svc.waitFootSteps()
00084 
00085 def demoGoPosQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="5. go pos in trot"):
00086     print >> sys.stderr, print_str
00087     # set gait type
00088     abcp=hcf.abc_svc.getAutoBalancerParam()[1]
00089     abcp.default_gait_type = gait_type
00090     hcf.abc_svc.setAutoBalancerParam(abcp)
00091     # set default translate pos
00092     ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00093     ggp.leg_default_translate_pos = [[0.0, -0.19, 0.0], [0.0, 0.19, 0.0], [0.7, -0.19, 0.0], [0.7, 0.19, 0.0]]
00094     hcf.abc_svc.setGaitGeneratorParam(ggp)
00095     # go pos
00096     hcf.abc_svc.goPos(-0.5, +0.2, 20)
00097     hcf.abc_svc.waitFootSteps()
00098 
00099 def demoGoVelocityQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="7. go velocity in trot"):
00100     print >> sys.stderr, print_str
00101     # set gait type
00102     abcp=hcf.abc_svc.getAutoBalancerParam()[1]
00103     abcp.default_gait_type = gait_type
00104     hcf.abc_svc.setAutoBalancerParam(abcp)
00105     # set default translate pos
00106     ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00107     ggp.leg_default_translate_pos = [[0.0, -0.19, 0.0], [0.0, 0.19, 0.0], [0.7, -0.19, 0.0], [0.7, 0.19, 0.0]]
00108     hcf.abc_svc.setGaitGeneratorParam(ggp)
00109     # go pos
00110     hcf.abc_svc.goVelocity(-0.1, -0.05, -20)
00111     time.sleep(3)
00112     hcf.abc_svc.goStop()
00113 
00114 def demo():
00115     init()
00116     demoGaitGeneratorSetFootSteps()
00117     demoGaitGeneratorSetFootStepsRectangle()
00118     demoGaitGeneratorSetFootStepsCycloidDelay()
00119     demoGaitGeneratorSetFootStepsCrawl()
00120     demoGoPosQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="5. go pos in trot")
00121     demoGoPosQuadruped(gait_type=OpenHRP.AutoBalancerService.PACE, print_str="6. go pos in pace")
00122     demoGoVelocityQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="7. go velocity in trot")
00123 
00124 if __name__ == '__main__':
00125     demo()