r(q_init)

q_init[0:7] = path_planner.q_init[0:7]

model.fullBody.setCurrentConfig(q_goal)
#~ r(q_goal)
q_goal = model.fullBody.generateContacts(q_goal, [0, 0, 1])
q_init = model.fullBody.generateContacts(q_init, [0, 0, 1])
#~ r(q_goal)

#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
model.fullBody.setStartState(q_init, [model.lLegId, rLegId])  #,rarmId,larmId])
fullBody.setEndState(q_goal, [rLegId, lLegId])  #,rarmId,larmId])

configs = d(0.01)
e(0.01)

from bezier_traj import *
init_bezier_traj(model.fullBody, r, pp, configs, model.limbsCOMConstraints)
#~ AFTER loading obstacles

com_vel = [0., 0., 0.]
com_acc = [0., 0., 0.]

vels = []
accs = []

#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
#~ gen(0,1)
Exemple #2
0
#~ from pickle import load
#~ q_init= load(f)
#~ f.close()
#~ r (q_init)

fullBody.setCurrentConfig(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
q_init = fullBody.generateContacts(q_init, [0, 0, 1])

fullBody.setStartState(q_init, [rLegId, lLegId, rarmId])  #,rarmId,larmId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId, rarmId]) #,rarmId,larmId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal, [rLegId, lLegId])  #,rarmId,larmId])

configs = d(0.005)
e()

qs = configs
fb = fullBody
ttp = path_planner
from bezier_traj import *
init_bezier_traj(fb, r, pp, qs, limbsCOMConstraints)
#~ AFTER loading obstacles
configs = qs
fullBody = fb
tp = ttp

#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
#~ gen(0,1)
Exemple #3
0
limbsCOMConstraints = { rLegId : {'file': "spiderman/RL_com.ineq", 'effector' : rfoot},  
						lLegId : {'file': "spiderman/LL_com.ineq", 'effector' : rHand},
						rarmId : {'file': "spiderman/RA_com.ineq", 'effector' : rHand},
						larmId : {'file': "spiderman/LA_com.ineq", 'effector' : lHand} }
						
						
ps = path_planner.ProblemSolver( fullBody )
r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
#~ fullBody.setJointBounds ("base_joint_xyz", [-1,3, -1, 1, 0, 2.2])
fullBody.setJointBounds ("base_joint_xyz", [-1,3, -1, 1, 0, 6])
pp = PathPlayer (fullBody.client.basic, r)

from plan_execute import a, b, c, d, e, init_plan_execute
init_plan_execute(fullBody, r, path_planner, pp)

q_0 = fullBody.getCurrentConfig(); 
q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]

fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
q_init = fullBody.generateContacts(q_init, [0,0,1])

fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])

configs = d(0.005); e()

from bezier_traj import *
init_bezier_traj(fullBody, r, pp, configs, limbsCOMConstraints)