# vim: foldmethod=marker foldlevel=2 from hpp.corbaserver.manipulation.hrp2 import Robot from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.gepetto.manipulation import Viewer, ViewerFactory from hpp.gepetto import PathPlayer, PathPlayerGui Robot.urdfSuffix = '_capsule_mesh' Robot.srdfSuffix = '_manipulation' # Load HRP2 {{{3 robot = Robot('hrp2', 'hrp2') ps = ProblemSolver(robot) vf = ViewerFactory(ps) robot.setJointBounds("hrp2/base_joint_xyz", [-0.2, 0.8, -0.5, 0.5, 0, 2]) ps.selectPathProjector('Progressive', 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterations(40) # 3}}} # Define configurations {{{3 half_sitting = q = robot.getInitialConfig() q_init = half_sitting[::] # Open hands ilh = robot.rankInConfiguration['hrp2/LARM_JOINT6'] q_init[ilh:ilh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] irh = robot.rankInConfiguration['hrp2/RARM_JOINT6'] q_init[irh:irh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ibjxyz = robot.rankInConfiguration['hrp2/base_joint_xyz']
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.gepetto.manipulation import Viewer class ScrewGun (object): rootJointType = 'freeflyer' packageName = 'airbus_environment' meshPackageName = 'airbus_environment' urdfName = 'screw_gun' urdfSuffix = "_massless" srdfSuffix = "" Robot.urdfSuffix = '_capsule_mesh' Robot.srdfSuffix = '_manipulation' # Load HRP2 and a screwgun {{{3 robot = Robot ('hrp2-screw', 'hrp2') ps = ProblemSolver (robot) ps.selectPathPlanner ("M-RRT") r = Viewer (ps) r.loadObjectModel (ScrewGun, 'screw_gun') r.buildCompositeRobot (['hrp2', 'screw_gun']) for d in ["hrp2", "screw_gun"]: robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4]) ps.selectPathOptimizer ('None') ps.setErrorThreshold (1e-3) ps.setMaxIterations (40) # 3}}} # Define configurations {{{3 half_sitting = q = robot.getInitialConfig ()