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):