# 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 ()
