Commit 1d304f21 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

knees and wrist

parent e12a589e
......@@ -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)
......@@ -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)
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment