def set_globals():
global robot_context
global states
global tp
global model
global path_planner
global ps
global r
global pp
global fullBody
global configs
global lLegId
global rLegId
global larmId
global rarmId
global limbsCOMConstraints
global fullBody
global configs
global cl
global path
states = robot_context["states"]
tp = robot_context["tp"]
model = robot_context["model"]
ps = robot_context["ps"]
r = robot_context["r"]
pp = robot_context["pp"]
fullBody = robot_context["fullBody"]
configs = robot_context["configs"]
cl = robot_context["cl"]
path = robot_context["path"]
limbsCOMConstraints = model.limbsCOMConstraints
rLegId = model.rLegId
lLegId = model.lLegId
larmId = model.larmId
rarmId = model.rarmId
path_planner = tp
init_plan_execute(fullBody, r, path_planner, pp)
init_bezier_traj(fullBody, r, pp, configs, limbsCOMConstraints)
import chair_path_2 as path_planner
import hrp2_model as model
#~ import hrp2_model_grasp as model
from hrp2_model import *
import time
ps = path_planner.ProblemSolver(model.fullBody)
r = path_planner.Viewer(ps, viewerClient=path_planner.r.client)
fullBody = model.fullBody
#~ 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(model.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_init)
q_init = [
-0.05,
-0.82,
0.55,
1.0,
0.0,
0.0,
'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_init)
#~ q_init = [
#~ -0.05, -0.82, 0.55, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
#~ 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
#~ 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
#~ 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
#~ 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
#~ 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
