Exemplo n.º 1
0
'''

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
Exemplo n.º 2
0
##
## 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)
Exemplo n.º 4
0
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)
Exemplo n.º 5
0
#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)
Exemplo n.º 6
0
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 --------------------------------------------------------------------