''' 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 --------------------------------------------------------------------