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)
#~ 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)
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)
