class ScrewGun (object): rootJointType = 'freeflyer' packageName = 'airbus_environment' meshPackageName = 'airbus_environment' urdfName = 'screw_gun' urdfSuffix = "" 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") vf = ViewerFactory (ps) vf.loadObjectModel (ScrewGun, 'screw_gun') for d in ["hrp2", "screw_gun"]: robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4]) 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 [::] # Set initial position of screw-driver # q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
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 () q_init = half_sitting [::] # Set initial position of screw-driver