hrpsys: samplespecialjointrobot_auto

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 ("SampleSpecialJointRobot(Robot)0", "$(PROJECT_DIR)/../model/sample_special_joint_robot.wrl")
00023     initial_pose = [0.0, -0.20944, -0.20944, 0.0, 0.523599, 0.523599, -0.314159, -0.314159, 0.0, 0.0,
00024                     0.0, -0.20944, -0.20944, 0.0, 0.523599, 0.523599, -0.314159, -0.314159, 0.0, 0.0,
00025                     0.0,      0.0,      0.0]
00026     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00027     hcf.waitInterpolation()
00028     hcf.startAutoBalancer()
00029     hrpsys_version = hcf.seq.ref.get_component_profile().version
00030     print("hrpsys_version = %s"%hrpsys_version)
00031 
00032 def checkActualBaseAttitude():
00033     rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation
00034     ret = math.degrees(rpy.r) < 0.1 and math.degrees(rpy.p) < 0.1
00035     print >> sys.stderr, "  actual base rpy = ", ret, "(", rpy, ")"
00036     assert (ret)
00037     return ret
00038 
00039 def demoGaitGeneratorNoToeHeelContact():
00040     print >> sys.stderr, "1. Do not use toe heel contact"
00041     hcf.abc_svc.goPos(0.3, 0, 0);
00042     hcf.abc_svc.waitFootSteps()
00043     checkActualBaseAttitude()
00044     print >> sys.stderr, "  No toe heel contact=>OK"
00045 
00046 def demoGaitGeneratorToeHeelContact():
00047     print >> sys.stderr, "2. Use toe heel contact"
00048     ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
00049     ggp.toe_pos_offset_x = 1e-3*182.0;
00050     ggp.heel_pos_offset_x = 1e-3*-72.0;
00051     ggp.toe_zmp_offset_x = 1e-3*182.0;
00052     ggp.heel_zmp_offset_x = 1e-3*-72.0;
00053     ggp.toe_angle = 35;
00054     ggp.heel_angle = 10;
00055     ggp.use_toe_joint = False;
00056     hcf.abc_svc.setGaitGeneratorParam(ggp);
00057     hcf.abc_svc.goPos(0.3, 0, 0);
00058     hcf.abc_svc.waitFootSteps()
00059     ggp.toe_angle = 0;
00060     ggp.heel_angle = 0;
00061     hcf.abc_svc.setGaitGeneratorParam(ggp);
00062     checkActualBaseAttitude()
00063     print >> sys.stderr, "  Toe heel contact=>OK"
00064 
00065 def demoGaitGeneratorToeHeelContactWithToeJoint():
00066     print >> sys.stderr, "3. Use toe heel contact with toe joint"
00067     ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
00068     ggp.toe_pos_offset_x = 1e-3*182.0;
00069     ggp.heel_pos_offset_x = 1e-3*-72.0;
00070     ggp.toe_zmp_offset_x = 1e-3*182.0;
00071     ggp.heel_zmp_offset_x = 1e-3*-72.0;
00072     ggp.toe_angle = 35;
00073     ggp.heel_angle = 10;
00074     ggp.use_toe_joint = True;
00075     hcf.abc_svc.setGaitGeneratorParam(ggp);
00076     hcf.abc_svc.goPos(0.3, 0, 0);
00077     hcf.abc_svc.waitFootSteps()
00078     ggp.toe_angle = 0;
00079     ggp.heel_angle = 0;
00080     hcf.abc_svc.setGaitGeneratorParam(ggp);
00081     checkActualBaseAttitude()
00082     print >> sys.stderr, "  Toe heel contact with toe joint =>OK"
00083 
00084 
00085 def demo():
00086     init()
00087     demoGaitGeneratorNoToeHeelContact()
00088     demoGaitGeneratorToeHeelContact()
00089     demoGaitGeneratorToeHeelContactWithToeJoint()
00090 
00091 if __name__ == '__main__':
00092     demo()