elif n.startswith ("talos/gripper_left"):
ps.createLockedJoint(n, n, half_sitting[r:r+s])
left_gripper_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r+s])
graph = ConstraintGraph.buildGenericGraph(robot, 'graph',
[ "talos/left_gripper", "talos/right_gripper", ],
[ "box", ],
[ Object.handles, ],
[ [ ], ],
[ ],
[ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True),
Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ]
)
graph.setConstraints (graph=True,
lockDof = left_gripper_lock + right_gripper_lock + other_lock,
numConstraints = [ "com_talos_box", "gaze"] + foot_placement)
graph.initialize()
res, q_init, err = graph.applyNodeConstraints("talos/left_gripper grasps box/top", half_sitting)
res, q_goal, err = graph.applyNodeConstraints("talos/right_gripper grasps box/bottom", half_sitting)
print ps.directPath(q_init, q_init, True)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.setParameter("SimpleTimeParameterization/safety", CORBA.Any(CORBA.TC_double, 0.5))
ps.setParameter("SimpleTimeParameterization/velocity", CORBA.Any(CORBA.TC_boolean, True))
ps.solve()
graph.addConstraints(
node="starting_state",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock))
graph.addConstraints(
node="free",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock +
gaze_constraint + waist_constraint))
graph.addConstraints(edge="Loop | f",
constraints=Constraints(numConstraints=com_constraint +
foot_placement + arm_locked))
graph.initialize()
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25)
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("SimpleTimeParameterization")
res, q, err = graph.generateTargetConfig("starting_motion", initConf, initConf)
if not res:
raise RuntimeError('Failed to project initial configuration')
ps.setRandomSeed(54)
ps.setInitialConfig(initConf)
ps.addGoalConfig(q)
ps.solve()
# Delete intermediate non optimized paths
for i in range(ps.numberPaths() - 1):
], [], [
Rule([
"talos/left_gripper",
], [
Object.handles[0],
], True),
Rule([
"talos/right_gripper",
], [
Object.handles[1],
], True),
])
graph.setConstraints(
graph=True,
lockDof=left_gripper_lock + right_gripper_lock + other_lock,
numConstraints=["com_talos_mire", "gaze"] + foot_placement)
graph.initialize()
res, q_init, err = graph.applyNodeConstraints(
"talos/left_gripper grasps mire/top", half_sitting)
res, q_goal, err = graph.applyNodeConstraints(
"talos/right_gripper grasps mire/bottom", half_sitting)
print ps.directPath(q_init, q_init, True)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
# ps.solve()
shrinkJointRange(robot, 0.6)
joint_bounds["grasp-generation"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
setRobotJointBounds("default")
shrinkJointRange(robot, 0.95)
joint_bounds["planning"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
ps.selectPathValidation("Graph-Discretized", 0.05)
#ps.selectPathValidation("Graph-Dichotomy", 0)
#ps.selectPathValidation("Graph-Progressive", 0.02)
ps.selectPathProjector("Progressive", 0.2)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
if isSimulation:
ps.setMaxIterProjection (1)
ps.setParameter("SimpleTimeParameterization/safety", 0.25)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05)
q0 = robot.getCurrentConfig()
q0[:4] = [-3., 2., 0, -1]
#q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.15
q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.34
q0[robot.rankInConfiguration['tiago/arm_1_joint']] = 0.10
q0[robot.rankInConfiguration['tiago/arm_2_joint']] = -1.47
q0[robot.rankInConfiguration['tiago/arm_3_joint']] = -0.16
q0[robot.rankInConfiguration['tiago/arm_4_joint']] = 1.87
q0[robot.rankInConfiguration['tiago/arm_5_joint']] = -1.57
q0[robot.rankInConfiguration['tiago/arm_6_joint']] = 1.3
robot.setJointBounds('tiago/root_joint', [-2, 2, -2, 2])
#robot.insertRobotSRDFModel("tiago", "tiago_data", "schunk", "_gripper")
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadRobotModel(Driller, "driller")
robot.insertRobotSRDFModel("driller", "gerard_bauzil", "qr_drill", "")
robot.setJointBounds('driller/root_joint', [-2, 2, -2, 2, 0, 2])
ps.selectPathValidation("Graph-Dichotomy", 0)
ps.selectPathProjector("Progressive", 0.2)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
if isSimulation:
ps.setMaxIterProjection(1)
ps.setParameter("SimpleTimeParameterization/safety", 0.25)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
#from hpp import Quaternion
#oMsk = (0.10576, -0.0168, 1.6835) + Quaternion().fromRPY(1.8, 0, 0).toTuple()
#oMsk = (0.30576, -0.0138, 1.5835) + Quaternion().fromRPY(1.8, 0, 0).toTuple()
#vf.loadObstacleModel(skinTagUrdf, "skin")
#vf.moveObstacle("skin", oMsk)
vf.loadObjectModel(AircraftSkin, "skin")
#vf.loadRobotModelFromString ("skin", AircraftSkin.rootJointType, AircraftSkin.urdfString, AircraftSkin.srdfString)
#robot.setRootJointPosition("skin", oMsk)
#robot.setJointPosition("skin/root_joint", oMsk)
shrinkJointRange(robot, 0.95)
