Commit 1500977e authored by stevet's avatar stevet
Browse files

fixed anymal slalom scripts + start state in rbprmFullBody

parent 85d46de9
......@@ -28,7 +28,7 @@ for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.boundSO3([-0.1,0.1,-0.5,0.5,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
c# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,-vMax,vMax,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
......@@ -39,7 +39,7 @@ ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("Kinodynamic/verticalAccelerationBound",aMaxZ)
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceOrientation",True)
ps.setParameter("Kinodynamic/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.1)
ps.setParameter("DynamicPlanner/sizeFootY",0.1)
ps.setParameter("DynamicPlanner/friction",mu)
......
......@@ -77,8 +77,8 @@ v.addLandmark('anymal/base',0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,fullBody.limbs_names)
fullBody.setEndState(q_goal,fullBody.limbs_names)
fullBody.setStartState(q_init,fullBody.limbs_names, [[0.,0.,1.] for _ in range(4)])
fullBody.setEndState(q_goal,fullBody.limbs_names, [[0.,0.,1.] for _ in range(4)])
print "Generate contact plan ..."
......
......@@ -37,7 +37,7 @@ ps = ProblemSolver( rbprmBuilder )
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceOrientation",True)
ps.setParameter("Kinodynamic/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
......
#!/bin/bash
gepetto-gui &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-gui'
pkill -f 'hpp-rbprm-server'
......@@ -285,7 +285,7 @@ class FullBody (Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName,configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample)
cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
return cl.setStartStateId(sId)
......@@ -303,7 +303,7 @@ class FullBody (Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName,configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample)
cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
return cl.setEndStateId(sId)
## Initialize the first state of the path interpolation
......
Markdown is supported
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