#~ fullBody.client.rbprm.rbprm.getOctreeTransform(larmId, q_0)
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
#~ q = fullBody.getCurrentConfig()
#~ q[0:confsize] = problem.configAtParam (0, t)[0:confsize]
#~ configs.append(q)
#~ t += dt
#~ i = i+1
#~
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
import datetime
now = datetime.datetime.now()
#~
f1 = open("polaris_hrp2"+str(now),"w+")
f1.write(str(configs))
f1.close()
#~ import hpp.gepetto.blender.exportmotion as em
#~ fullBody.exportAll(r, configs, "polaris_hrp2_OK"+str(now));
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ r(q_goal)
fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
#~ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand},
larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
from hpp.gepetto import PathPlayer
#~ pp = PathPlayer (ps.robot.client.basic, r, 0.001, 0.1)
pp = PathPlayer (ps.robot.client.basic, r)
#~ (self, client, publisher, dt=0.01, speed=1)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
]
r(q_init)
fullBody.setCurrentConfig(q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
#~ r(q_goal)
fullBody.setStartState(q_init, [rLegId, lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal, [rLegId, lLegId]) #,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0
fullBody.draw(configs[i], r)
i = i + 1
i - 1
#~ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
limbsCOMConstraints = {
rLegId: {
'file': "hrp2/RL_com.ineq",
'effector': 'RLEG_JOINT5'
},
lLegId: {
'file': "hrp2/LL_com.ineq",
'effector': 'LLEG_JOINT5'
},
rarmId: {
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)
# RB: Is this line redundent?
r.loadObstacleModel ('hpp-rbprm-corba', name_of_scene, "contact")
i = 0;
# fullBody.draw(configs[i],r); i=i+1; i-1
while (i < len(configs)):
fullBody.draw(configs[i],r)
sleep(0.1)
i = i + 1
print "Animation finished!"
