#~ f = open("scale_1_save","r+")
#~ 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)
]
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)
s = max(norm(array(states[i + 1].q()) - array(states[i].q())),
1.) * 0.6
path += [go0(states[-2:], num_optim=1, mu=mu, use_kin=context == 0)]
except:
global states
states = states[:-1]
init_bezier_traj(fullBody, r, pp, configs, limbsCOMConstraints)
all_paths = [[], []]
from hpp.corbaserver.rbprm.state_alg import *
#~ d(0.07);e(0.01)
i = 0
#~ d(0.09); e(0.01);
configs = d(0.1)
e()
states = planToStates(fullBody, configs)[:-1]
s = State(fullBody,
q=states[-1].q(),
limbsIncontact=states[-1].getLimbsInContact())
x = s.getCenterOfMass()
x[0] -= 0.05
x[2] -= 0.1
fullBody.projectStateToCOM(s.sId, x, 10)
r(s.q())
states += [s]
#~ lc()
#~ le = min(38, len(states)-10)
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)
0.0, # LLEG 25-30
0.0,
0.0,
-0.453785606,
0.872664626,
-0.41887902,
0.0, # RLEG 31-36
]
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.setStartState(q_init,[lLegId,rLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal, [rLegId, lLegId]) #,rarmId,larmId])
configs = d(0.005, filt=True)
e()
from bezier_traj import *
init_bezier_traj(model.fullBody, r, pp, configs, model.limbsCOMConstraints)
#~ AFTER loading obstacles
#~ 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)
0.872664626,
-0.41887902,
0.0, # RLEG 31-36
]
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.06, True)
e()
from bezier_traj import go0, go2, init_bezier_traj, reset
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import play_trajectory
import time
from hpp.corbaserver.rbprm.rbprmstate import State
from hpp.corbaserver.rbprm.state_alg import *
path = []
def sc(ec):
pass
