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()
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", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 2.0)
ps.solve()
def createInitializationGraph(name):
graph = ConstraintGraph.buildGenericGraph(
robot,
name,
["talos/left_gripper", "talos/right_gripper"],
["box"],
for q in q_proj_list:
robot.setCurrentConfig(q)
oMc = Transform(robot.getJointPosition("talos/rgbd_rgb_optical_joint"))
oMm = Transform(robot.getJointPosition("mire/root_joint"))
cMm = oMc.inverse() * oMm
hpp_poses.append(cMm.toTuple())
res, hs_proj, err = graph.applyNodeConstraints(
"talos/left_gripper grasps mire/left", half_sitting)
paths = list()
failed = False
for i, (q1,
q2) in enumerate(zip([hs_proj] + q_proj_list,
q_proj_list + [hs_proj])):
res, pid, msg = ps.directPath(q1, q2, True)
if res:
print("Path from", i, "to", i + 1, ":", pid)
paths.append(pid)
else:
print("Could not joint", i, "to", i + 1, ":", msg)
failed = True
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
cleanPaths = True
if not failed:
# join path
i0 = paths[0]
ps.createTransformationConstraint(
"hand_pose_2",
"talos/root_joint",
"talos/gripper_right_joint",
[0.6, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
[True, True, True, False, False, False],
)
ps.resetConstraints()
ps.setNumericalConstraints("proj", foot_placement + ["hand_pose_2"])
ps.setLockedJointConstraints(
"proj", left_gripper_lock + right_gripper_lock + other_lock)
res, qproj2, err = ps.applyConstraints(q_init)
paths = list()
res, pid, msg = ps.directPath(q_init, qproj1, True)
paths.append(pid)
res, pid, msg = ps.directPath(qproj1, qproj2, True)
paths.append(pid)
res, pid, msg = ps.directPath(qproj2, q_init, True)
paths.append(pid)
# Apply to half_sitting
# Look if it's ok
#
# Do the same with the second position of the gripper
# directPath between the two ?
# Test with servo off
# Test
# Profit ?
