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