Commit 3a5ca8d9 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

finally good normals for contacts

parent 1d304f21
......@@ -26,24 +26,24 @@ r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
#~ AFTER loading obstacles
rLeg = 'RLEG_JOINT0'
rKnee = 'RLEG_JOINT3'
rLegOffset = [0.055,0.105,0.017]
rLegNormal = [0,-1,0]
rLegOffset = [0.105,0.055,0.017]
rLegNormal = [-1,0,0]
rLegx = 0.05; rLegy = 0.05
fullBody.addLimb(rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 5000, 0.01)
lLeg = 'LLEG_JOINT0'
lKnee = 'LLEG_JOINT3'
lLegOffset = [0.105,0.055,-0.017]
lLegNormal = [0,-1,0]
lLegOffset = [0.105,0.055,0.017]
lLegNormal = [-1,0,0]
lLegx = 0.05; lLegy = 0.05
fullBody.addLimb(lLeg,lKnee,lLegOffset,rLegNormal, lLegx, lLegy, 5000, 0.01)
fullBody.addLimb(lLeg,lKnee,lLegOffset,lLegNormal, 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]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 5000, 0.01)
......@@ -52,7 +52,7 @@ fullBody.addLimb(rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 5000, 0.01)
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [0.03,-0.050,-0.050]
lArmNormal = [0,1,0]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 5000, 0.01)
......@@ -67,17 +67,45 @@ 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 [0:3] = [0, -0.5, 0.3]; fullBody.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [0.1, -0.5, 0.3]
#~ ps.setInitialConfig (q_init)
#~ ps.addGoalConfig (q_goal)
#~ r(q_goal)
#~ ps.solve ()
#~
#~ from hpp.gepetto import PathPlayer
#~ pp = PathPlayer (fullBody.client.basic, r)
#~ pp (0)
q_init = fullBody.generateContacts(q_init, [0,0,-1])
r (q_init)
#~ fullBody.setCurrentConfig (q_goal)
#~ q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
#~
#~ fullBody.setStartState(q_init,[rLeg,lLeg,rarm,larm])
#~ fullBody.setEndState(q_goal,[rLeg,lLeg,rarm,larm])
#~
#~ configs = fullBody.interpolate(0.005)
#~ i = 0;
#~ r (configs[i]); i=i+1; i-1
#~ 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)
#~ 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]
#~
#~ q_goal = q_init [::]
#~ q_goal [0:3] = [1, -0.5, 0.6]
#~ r (q_0)
#~ fullBody.setCurrentConfig (q_0)
......
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