knees and wrist

script/loadfullbody.py

@@ -33,7 +33,7 @@ fullBody.setCurrentConfig (q_init)
 q_init = fullBody.generateContacts(q_init, [0,1,0])
 r (q_init)
 
-fullBody.getContactSamplesIds("r_shoulder_pan_joint", q_init, [0,1,0])
+fullBody.getContactSamplesIds(rLeg, q_init, [0,1,0])
 
 #~ q_init = fullBody.getSample('r_shoulder_pan_joint', 1)
 #~ r (q_init)

script/loadhrp2crawl.py

@@ -26,33 +26,67 @@ r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
 #~ AFTER loading obstacles
 rLeg = 'RLEG_JOINT0'
 rKnee = 'RLEG_JOINT3'
-rLegOffset = [0.105,0.055,-0.017]
-rLegNormal = [1,0,0]
+rLegOffset = [0.055,0.105,0.017]
+rLegNormal = [0,-1,0]
 rLegx = 0.05; rLegy = 0.05
 fullBody.addLimb(rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 5000, 0.01)
 
 lLeg = 'LLEG_JOINT0'
-lKnee = 'RLEG_JOINT0'
+lKnee = 'LLEG_JOINT3'
 lLegOffset = [0.105,0.055,-0.017]
-lLegNormal = [1,0,0]
+lLegNormal = [0,-1,0]
 lLegx = 0.05; lLegy = 0.05
-#~ fullBody.addLimb(lLeg,lKnee,lLegOffset,rLegNormal, lLegx, lLegy, 5000, 0.01)
+fullBody.addLimb(lLeg,lKnee,lLegOffset,rLegNormal, lLegx, lLegy, 5000, 0.01)
+#~  	
+
+#~ AFTER loading obstacles
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0.03,-0.050,-0.050]
+rArmNormal = [0,1,0]
+rArmx = 0.024; rArmy = 0.024
+fullBody.addLimb(rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 5000, 0.01)
+
+
+#~ AFTER loading obstacles
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [0.03,-0.050,-0.050]
+lArmNormal = [0,1,0]
+lArmx = 0.024; lArmy = 0.024
+fullBody.addLimb(larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 5000, 0.01)
+
+
+q_0 = fullBody.getCurrentConfig (); r (q_0)
 
 fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
-fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[1])
 fullBody.client.basic.robot.setJointConfig('LLEG_JOINT3',[1.5])
 fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
+#~ 
+fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
 
-
-q_init = fullBody.getCurrentConfig ()
-q_init [0:3] = [0, -0.5, 0.6]
-r (q_init)
+q_init = fullBody.getCurrentConfig (); r (q_init)
+q_init [0:3] = [0, -0.5, 0.2]; fullBody.setCurrentConfig (q_init); r (q_init)
+#~ configs = fullBody.getContactSamplesIds(rLeg, q_init, [0,1,0])
+#~ i = 0
+#~ q_init = fullBody.getSample(rLeg, int(configs[i])); i = i+1;r(q_init)
+#~ 
+#~ fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [-0.1,0,1]) ; r(q_init)
+#~ r (q_init)
 
 q_goal = q_init [::]
 q_goal [0:3] = [1, -0.5, 0.6]
 
-#~ 
-#~ fullBody.setCurrentConfig (q_init)
-#~ q_init = fullBody.generateContacts(q_init, [0,0,-1])
-#~ r (q_init)
+#~ r (q_0)
+#~ fullBody.setCurrentConfig (q_0)
+#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT3',[1.5])
+#~ fullBody.client.basic.robot.setJointConfig('RLEG_JOINT3',[1.5])
 
+#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
+#~ q_init = fullBody.getCurrentConfig (); r (q_init)
+#~ q_init [0:3] = [0, -0.5, 0.2]; fullBody.setCurrentConfig (q_init); r (q_init)
+#~ q_init = fullBody.generateContacts(q_init, [-0.1,0,1]) ; r(q_init)

src/rbprmbuilder.impl.cc

@@ -240,7 +240,7 @@ namespace hpp {
                                           + std::string(limbname) + " to robot; limb not defined exists");
             }
             const RbPrmLimbPtr_t& limb = lit->second;
-            fcl::Transform3f transform = limb->limb_->robot()->rootJoint()->currentTransformation (); // get root transform from configuration
+            fcl::Transform3f transform = limb->limb_->robot()->rootJoint()->childJoint(0)->currentTransformation (); // get root transform from configuration
             std::vector<sampling::T_OctreeReport> reports(problemSolver_->collisionObstacles().size());
             std::size_t i (0);
             //#pragma omp parallel for