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)