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)
