'''
import time
import numpy as np
from numpy.linalg import norm
from pinocchio import SE3
from pinocchio.romeo_wrapper import RomeoWrapper
from pinocchio.utils import eye, se3ToXYZQUAT
from scipy.optimize import fmin_bfgs
# --- Load robot model
pkg = 'models/romeo/'
urdf = pkg + 'urdf/romeo.urdf'
robot = RomeoWrapper(urdf, [
pkg,
])
robot.initDisplay(loadModel=True)
if 'viewer' not in robot.__dict__:
robot.initDisplay()
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
robot.viewer.gui.addSphere("world/green", .05,
[.2, 1., 2., .5]) # .1 is the radius
## ## Copyright (c) 2018-2019 CNRS INRIA ## import pinocchio as pin pin.switchToNumpyMatrix() from pinocchio.romeo_wrapper import RomeoWrapper ## Load Romeo with RomeoWrapper import os current_path = os.getcwd() # The model of Romeo is contained in the path PINOCCHIO_GIT_REPOSITORY/models/romeo model_path = current_path + "/" + "../../models/romeo" mesh_dir = model_path urdf_filename = "romeo_small.urdf" urdf_model_path = model_path + "/romeo_description/urdf/" + urdf_filename robot = RomeoWrapper(urdf_model_path, [mesh_dir]) ## alias model = robot.model data = robot.data ## do whatever, e.g. compute the center of mass position in the world frame q0 = robot.q0 com = robot.com(q0) # This last command is similar to com2 = pin.centerOfMass(model, data, q0)
import pinocchio as se3
from pinocchio.romeo_wrapper import RomeoWrapper
from pinocchio.utils import *
from numpy.linalg import norm
path = '/home/nmansard/src/pinocchio/pinocchio/models/romeo/'
urdf = path + 'urdf/romeo.urdf'
pkgs = [
path,
]
robot = RomeoWrapper(urdf, pkgs, se3.JointModelFreeFlyer()) # Load urdf model
# The robot is loaded with the basis fixed to the world
robot.initDisplay(loadModel=True) # setup the viewer to display the robot
NQ = robot.model.nq # model configuration size (6)
NV = robot.model.nv # model configuration velocity size (6)
q = rand(NQ) # Set up an initial configuration
q[3:7] /= norm(q[3:7]) # Normalize the quaternion
robot.display(q)
vq = zero(NV)
vq[10] = 1 # Set up a constant robot speed
from time import sleep
for i in range(10000): # Move the robot with constant velocity
q[7:] += vq[6:] / 100
robot.display(q)
sleep(.01)
WITH_UR5 = True
WITH_ROMEO = False
if WITH_UR5:
from pinocchio.robot_wrapper import RobotWrapper
path = 'models/'
urdf = path + '/ur_description/urdf/ur5_gripper.urdf'
robot = RobotWrapper(urdf, [
path,
])
if WITH_ROMEO:
from pinocchio.romeo_wrapper import RomeoWrapper
path = 'models/romeo/'
urdf = path + 'urdf/romeo.urdf'
# Explicitly specify that the first joint is a free flyer.
robot = RomeoWrapper(urdf, [
path,
]) # Load urdf model
robot.initDisplay(loadModel=True)
robot.display(robot.q0)
#Romeo Test from pinocchio.romeo_wrapper import RomeoWrapper from os.path import join import pinocchio as se3 from pinocchio.utils import * import numpy as np import scipy as sp import time PKG = '/opt/openrobots/share' URDF = join(PKG, 'romeo_description/urdf/romeo.urdf') robot = RomeoWrapper(URDF, [PKG]) robot.initDisplay(loadModel=True) q = rand(robot.nq) se3.forwardKinematics(robot.model, robot.data, q) robot.display(q) q = rand(robot.nq) se3.forwardKinematics(robot.model, robot.data, q) robot.display(q)
set_printoptions(suppress=True, precision=7)
#-----------------------------------------------------------------------------
#---- ROBOT SPECIFICATIONS----------------------------------------------------
#-----------------------------------------------------------------------------
#Define robotName, urdfpath and initialConfig
#Taking input from pinocchio
from pinocchio.romeo_wrapper import RomeoWrapper
import pinocchio as pin
#SET THE PATH TO THE URDF AND MESHES
urdfPath = "~/git/sot/pinocchio/models/romeo.urdf"
urdfDir = ["~/git/sot/pinocchio/models"]
pinocchioRobot = RomeoWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
robotName = 'romeo'
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(pinocchioRobot.q0)
initialConfig = (0, 0, .840252, 0, 0, 0, # FF
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # RLEG
0, # TRUNK
1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # LARM
0, 0, 0, 0, # HEAD
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM
)
#-----------------------------------------------------------------------------
#---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------