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 ?