def loadHRP(self, urdf, pkgs, loadModel):
'''Internal: load HRP-2 model from URDF.'''
robot = RobotWrapper(urdf, pkgs, root_joint=se3.JointModelFreeFlyer())
robot.initDisplay(loadModel=loadModel)
if 'viewer' not in robot.__dict__: robot.initDisplay()
for n in robot.visual_model.geometryObjects:
robot.viewer.gui.setVisibility(robot.viewerNodeNames(n), 'OFF')
robot.q0 = np.matrix([
0.,
0.,
0.648702,
0.,
0.,
0.,
1., # Free flyer
0.,
0.,
0.,
0., # Chest and head
0.261799,
0.17453,
0.,
-0.523599,
0.,
0.,
0.1, # Left arm
0.261799,
-0.17453,
0.,
-0.523599,
0.,
0.,
0.1, # Right arm
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Left leg
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Righ leg
]).T
self.hrpfull = robot
import time
from numpy.linalg import inv
from math import pi
from scipy.optimize import fmin_bfgs, fmin_slsqp
# --- Load robot model
pkg = 'models/'
urdf = pkg + 'ur_description/urdf/ur5_gripper.urdf'
robot = RobotWrapper(urdf, [
pkg,
])
robot.initDisplay(loadModel=True)
if 'viewer' not in robot.__dict__: robot.initDisplay()
# Define an init config
robot.q0 = np.matrix([0, -pi / 2, 0, 0, 0, 0])
NQ = robot.model.nq
NV = robot.model.nv
# --- Add ball to represent target
robot.viewer.gui.addSphere("world/red", .05,
[1., .2, .2, .5]) # .1 is the radius
robot.viewer.gui.addSphere("world/blue", .05,
[.2, .2, 1., .5]) # .1 is the radius
# Shortcut function to convert SE3 to 7D vector.
M2gv = lambda M: se3ToXYZQUAT(M)
def place(objectId, M):
