samplerobot_collision_detector.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 def vector_equal_eps (vec1, vec2, eps=1e-5):
00017     if len(vec1) == len(vec2):
00018         for e1, e2 in zip(vec1, vec2):
00019             if abs(e1 - e2) > eps:
00020                 return False
00021         return True
00022     else:
00023         return False
00024 
00025 def init ():
00026     global hcf, init_pose, col_safe_pose, col_fail_pose, hrpsys_version
00027     hcf = HrpsysConfigurator()
00028     hcf.getRTCList = hcf.getRTCListUnstable
00029     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00030     init_pose = [0]*29
00031     col_safe_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,0.15708,-0.113446,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0]
00032     col_fail_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.845363,0.03992,0.250074,-1.32816,0.167513,0.016204,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0]
00033     hrpsys_version = hcf.co.ref.get_component_profile().version
00034     print >> sys.stderr, "hrpsys_version = %s"%hrpsys_version
00035 
00036 # demo functions
00037 def demoCollisionCheckSafe ():
00038     print >> sys.stderr, "1. CollisionCheck in safe pose"
00039     hcf.seq_svc.setJointAngles(col_safe_pose, 3.0);
00040     hcf.waitInterpolation();
00041     counter = 0
00042     while (counter < 20) and (not vector_equal_eps([x / 180 * math.pi  for x in hcf.getJointAngles()], col_safe_pose)):
00043         time.sleep(0.2)
00044         counter = counter + 1
00045     assert(counter != 20)
00046     cs=hcf.co_svc.getCollisionStatus()[1]
00047     if cs.safe_posture:
00048         print >> sys.stderr, "  => Safe pose"
00049     assert(cs.safe_posture is True)
00050 
00051 def demoCollisionCheckFail ():
00052     print >> sys.stderr, "2. CollisionCheck in fail pose"
00053     hcf.seq_svc.setJointAngles(col_fail_pose, 3.0);
00054     hcf.waitInterpolation();
00055     cs=hcf.co_svc.getCollisionStatus()[1]
00056     if not cs.safe_posture:
00057         print >> sys.stderr, "  => Successfully stop fail pose"
00058     assert((not cs.safe_posture) is True)
00059     hcf.seq_svc.setJointAngles(col_safe_pose, 3.0);
00060     hcf.waitInterpolation();
00061     cs=hcf.co_svc.getCollisionStatus()[1]
00062     if cs.safe_posture:
00063         print >> sys.stderr, "  => Successfully return to safe pose"
00064     assert(cs.safe_posture is True)
00065 
00066 def demoCollisionCheckFailWithSetTolerance ():
00067     print >> sys.stderr, "3. CollisionCheck in fail pose with 0.1[m] tolerance"
00068     hcf.co_svc.setTolerance("all", 0.1); # [m]
00069     hcf.seq_svc.setJointAngles(col_fail_pose, 1.0);
00070     hcf.waitInterpolation();
00071     cs=hcf.co_svc.getCollisionStatus()[1]
00072     if not cs.safe_posture:
00073         print >> sys.stderr, "  => Successfully stop fail pose (0.1[m] tolerance)"
00074     assert((not cs.safe_posture) is True)
00075     hcf.co_svc.setTolerance("all", 0.0); # [m]
00076     hcf.seq_svc.setJointAngles(col_safe_pose, 3.0);
00077     hcf.waitInterpolation();
00078     cs=hcf.co_svc.getCollisionStatus()[1]
00079     if cs.safe_posture:
00080         print >> sys.stderr, "  => Successfully return to safe pose"
00081     assert(cs.safe_posture is True)
00082 
00083 def demoCollisionDisableEnable ():
00084     print >> sys.stderr, "4. CollisionDetection enable and disable"
00085     hcf.seq_svc.setJointAngles(col_safe_pose, 1.0);
00086     hcf.waitInterpolation();
00087     if hcf.co_svc.disableCollisionDetection():
00088         print >> sys.stderr, "  => Successfully disabled when no collision"
00089     assert(hcf.co_svc.disableCollisionDetection() is True)
00090     if hcf.co_svc.enableCollisionDetection():
00091         print >> sys.stderr, "  => Successfully enabled when no collision"
00092     assert(hcf.co_svc.enableCollisionDetection() is True)
00093     hcf.seq_svc.setJointAngles(col_fail_pose, 1.0);
00094     hcf.waitInterpolation();
00095     if not hcf.co_svc.disableCollisionDetection():
00096         print >> sys.stderr, "  => Successfully inhibit disabling when collision"
00097     assert((not hcf.co_svc.disableCollisionDetection()) is True)
00098     hcf.seq_svc.setJointAngles(col_safe_pose, 1.0);
00099     hcf.waitInterpolation();
00100 
00101 def demoCollisionMask ():
00102     if hcf.abc_svc != None:
00103         print >> sys.stderr, "5. Collision mask test"
00104         hcf.co_svc.setTolerance("all", 0); # [m]
00105         hcf.startAutoBalancer()
00106         print >> sys.stderr, "  5.1 Collision mask among legs : Check RLEG_ANKLE_R - LLEG_ANKLE_R"
00107         print >> sys.stderr, "      Desired behavior : Robot stops when legs collision."
00108         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]),
00109                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])])
00110         hcf.abc_svc.waitFootSteps();
00111         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]),
00112                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")])])
00113         hcf.abc_svc.waitFootSteps();
00114         print >> sys.stderr, "  => Successfully mask works. Legs joints stops when collision."
00115         print >> sys.stderr, "  5.2 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST*"
00116         print >> sys.stderr, "      Desired behavior : Leg joints moves and arm joints stops when collision."
00117         hcf.seq_svc.setJointAngles(col_safe_pose, 1.0);
00118         hcf.waitInterpolation();
00119         hcf.abc_svc.goVelocity(0,0,0);
00120         hcf.seq_svc.setJointAngles(col_fail_pose, 1.0);
00121         hcf.waitInterpolation();
00122         hcf.seq_svc.setJointAngles(col_safe_pose, 3.0);
00123         hcf.waitInterpolation();
00124         hcf.abc_svc.goStop();
00125         print >> sys.stderr, "  => Successfully mask works. Arm joints stops and leg joints moves."
00126         print >> sys.stderr, "  5.3 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST* and RLEG_ANKLE_R and LLEG_ANKLE_R (combination of 5.1 and 5.2)"
00127         print >> sys.stderr, "      Desired behavior : First, arm stops and legs moves."
00128         hcf.seq_svc.setJointAngles(col_safe_pose, 1.0);
00129         hcf.waitInterpolation();
00130         print >> sys.stderr, "      Desired behavior : Next, arm keeps stopping and legs stops."
00131         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]),
00132                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])])
00133         hcf.abc_svc.waitFootSteps();
00134         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]),
00135                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")])])
00136         hcf.abc_svc.waitFootSteps();
00137         print >> sys.stderr, "  => Successfully mask works with combined situation."
00138         hcf.stopAutoBalancer()
00139 
00140 def demo():
00141     init()
00142     demoCollisionCheckSafe()
00143     demoCollisionCheckFail()
00144     demoCollisionCheckFailWithSetTolerance()
00145     demoCollisionDisableEnable()
00146     #demoCollisionMask()
00147 
00148 def demo_co_loop():
00149     init()
00150     from distutils.version import StrictVersion
00151     if StrictVersion(hrpsys_version) >= StrictVersion('315.10.0'):
00152         demoCollisionCheckSafe()
00153         demoCollisionCheckFail()
00154         demoCollisionCheckFailWithSetTolerance()
00155         demoCollisionDisableEnable()
00156 
00157 if __name__ == '__main__':
00158     demo()


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