import numpy as np import pinocchio as pin from pinocchio.utils import * from os.path import join from pinocchio.romeo_wrapper import RomeoWrapper from pinocchio.robot_wrapper import RobotWrapper from scipy.optimize import fmin_bfgs, fmin_slsqp from time import sleep from IPython import embed #embed makes the programm runs till embed() # Loading the robot PKG = '/opt/openrobots/share' #URDF = join(PKG, 'romeo_description/urdf/romeo.urdf') URDF = join(PKG, 'romeo_description/urdf/romeo_small.urdf') robot = RomeoWrapper.BuildFromURDF(URDF, [PKG]) print 'nq = ', robot.model.nq print 'nv = ', robot.model.nv # Display default position of the robot robot.initDisplay(loadModel=True) # Cost function def cost(x): pin.forwardKinematics(robot.model, robot.data, np.matrix(x).T) p = robot.data.oMi[-1].translation norm_diff = np.linalg.norm(pdes - p.getA()[:, 0]) return norm_diff
WITH_UR5 = True WITH_ROMEO = False if WITH_UR5: from pinocchio.robot_wrapper import RobotWrapper urdf = '/opt/openrobots/share/ur5_description/urdf/ur5_gripper.urdf' robot = RobotWrapper.BuildFromURDF(urdf) if WITH_ROMEO: from pinocchio.romeo_wrapper import RomeoWrapper urdf = '/opt/openrobots/share/romeo_description/urdf/romeo.urdf' robot = RomeoWrapper.BuildFromURDF(urdf) robot.initDisplay(loadModel=True) robot.display(robot.q0)