def makeRobotProblemAndViewerFactory(clients, rolling_table=True): objects = list() robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer", client=clients) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2]) shrinkJointRange (robot, 0.95) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) objects.append(Box(name="box", vf=vf)) robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2]) # Loaded as an object to get the visual tags at the right position. # vf.loadEnvironmentModel (Table, 'table') if rolling_table: table = RollingTable(name="table", vf=vf) else: table = Table(name="table", vf=vf) robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2]) return robot, ps, vf, table, objects
def makeRobotProblemAndViewerFactory(corbaclient): robot = Robot("dev", "talos", rootJointType="freeflyer", client=corbaclient) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver(robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) vf.loadObjectModel(Object, "box") robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2]) vf.loadEnvironmentModel(Table, "table") return robot, ps, vf
def makeRobotProblemAndViewerFactory(clients, rolling_table=True, rosParam=None): if rosParam is not None: import rospy, os, hpp HumanoidRobot.urdfString = rospy.get_param(rosParam) srdfFilename = hpp.retrieveRosResource(HumanoidRobot.srdfFilename) with open(srdfFilename, 'r') as f: HumanoidRobot.srdfString = f.read() objects = list() robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer", client=clients) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2]) shrinkJointRange(robot, 0.95) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") ps.selectPathValidation('Graph-Progressive', 0.01) vf = ViewerFactory(ps) objects.append(Box(name="box", vf=vf)) robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2]) # Loaded as an object to get the visual tags at the right position. # vf.loadEnvironmentModel (Table, 'table') if rolling_table: table = RollingTable(name="table", vf=vf) else: table = Table(name="table", vf=vf) robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2]) return robot, ps, vf, table, objects
srdfSuffix = "" handles = [ "box/top", "box/bottom" ] robot = Robot ('dev', 'talos', rootJointType = "freeflyer") robot. leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver (robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold (1e-3) ps.setMaxIterProjection (40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory (ps) Object = Box vf.loadObjectModel (Object, 'box') robot.setJointBounds ("box/root_joint", [-1, 1, -1, 1, 0, 2]) qq = [-0.7671778026566639, 0.0073267002287253635, 1.0035168727631776, -0.003341673452654457, -0.021566597515109524, 0.0002183620894239602, 0.9997618053357284, -0.00020053128587844821, -0.0021365695604276275, -0.4415951773681094, 0.9659230706255528, -0.48119003672520416, 0.007109157982067145, -0.00020095991543181877, -0.002126639473414498, -0.4382848597339842, 0.9589221865248464, -0.4774994711722908, 0.007099218648561522, 0.0, 0.025235347910697536, -0.06985947194357875, 0.0, -0.12446173084176845, -1.5808415926365578, 0.014333078135875619, -0.0806417043955706, -0.37124401660668394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25955282922987977, -0.1618313202464181, 0.002447883426630002, -0.5149037074691503, -0.00010703274362664899, 0.0008742582163227642, 0.10168585913285667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398163397, 0.32250041955468695, -0.2569883469655496, 0.18577095561452217, 1.164388709412583, 0.0694401264431558, 0.5475575114527793, -0.11842286843715424, 0.8254301089264399] half_sitting = [ -0.74,0,1.0192720229567027,0,0,0,1, # root_joint 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right 0, 0.006761, # torso 0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left 0, 0, 0, 0, 0, 0, 0, # gripper_left
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): ps.erasePath(0) # Generate N random configurations
def setRobotJointBounds(which): for jn, bound in joint_bounds[which]: robot.setJointBounds(jn, bound) joint_bounds["default"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] 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
# create intermediate touchpoints configuration success3, q_touch_1, residual_error3 = graph.applyNodeConstraints("talos/right_gripper grasps desk/touchpoint_right_top", q_init) # Collision between legs success4, q_touch_2, residual_error4 = graph.applyNodeConstraints("talos/right_gripper grasps desk/touchpoint_right_front", q_init) # Collision between legs success1, q_touch_3, residual_error1 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_front", q_init) # Collision between legs success2, q_touch_4, residual_error2 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_drawer", q_init) # fail # q_touch_1 redefinition q_touch_1 = coda.q_touch_1 # q_touch_2 redefinition q_touch_2 = coda.q_touch_2 # q_touch_4 redefinition q_touch_4 = coda.q_touch_4 ps.addPathOptimizer("Graph-RandomShortcut") from hpp.gepetto import PathPlayer pp = PathPlayer(v, ps.client.basic) def solveAll(): # trajectory 1 ps.setInitialConfig(q_init) ps.addGoalConfig(q_touch_1) graph.setWeight("talos/right_gripper > desk/touchpoint_right_front | f", 0) graph.setWeight("talos/left_gripper > desk/touchpoint_left_front | f", 0) graph.setWeight("talos/left_gripper > desk/touchpoint_left_drawer | f", 0) graph.setWeight("talos/right_gripper > desk/upper_drawer_spherical_handle | f", 0) print ps.solve() # trajectory 2