def __init__(self):
self.robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo.urdf")
self.robot.initDisplay()
self.robot.loadDisplayModel("world/pinocchio","pinocchio")
self.robot.display(self.robot.q0)
self.robot.viewer.gui.refresh()
self.q=np.copy(self.robot.q0)
def __init__(self, dt):
self.dt = dt
self.robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo.urdf")
self.robot.initDisplay()
self.robot.loadDisplayModel("world/pinocchio", "pinocchio")
self.robot.display(self.robot.q0)
self.robot.viewer.gui.refresh()
self.q = np.copy(self.robot.q0)
self.v = np.copy(self.robot.v0)
self.a = np.copy(self.robot.v0)
self.dq = np.matrix(np.zeros([self.robot.nv, 1]))
## ## 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)
import pinocchio as pin from pinocchio.romeo_wrapper import RomeoWrapper DISPLAY = False ## 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) ## load model into gepetto-gui if DISPLAY: import gepetto.corbaserver
class PinocchioControllerAcceleration(object):
def __init__(self, dt):
self.dt = dt
self.robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo.urdf")
self.robot.initDisplay()
self.robot.loadDisplayModel("world/pinocchio", "pinocchio")
self.robot.display(self.robot.q0)
self.robot.viewer.gui.refresh()
self.q = np.copy(self.robot.q0)
self.v = np.copy(self.robot.v0)
self.a = np.copy(self.robot.v0)
self.dq = np.matrix(np.zeros([self.robot.nv, 1]))
def controlLfRfCom(
self,
Lf=[0.0, 0.0, 0.0],
dLf=[0.0, 0.0, 0.0],
Rf=[0.0, 0.0, 0.0],
dRf=[0.0, 0.0, 0.0],
Com=[0, 0, 0.63],
dCom=[0.0, 0.0, 0.0],
):
def robotint(q, dq):
M = se3.SE3(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix(), q[:3])
dM = se3.exp(dq[:6])
M = M * dM
q[:3] = M.translation
q[3:7] = se3.Quaternion(M.rotation).coeffs()
q[7:] += dq[6:]
def robotdoubleint(q, dq, ddq, dt):
dq += dt * ddq
robotint(q, dq)
def errorInSE3(M, Mdes):
"""
Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
between M and Mdes, both element of SE3.
"""
error = se3.log(Mdes.inverse() * M)
return error.vector()
def errorInSE3dyn(M, Mdes, v_frame, v_des):
gMl = se3.SE3.Identity()
gMl.rotation = M.rotation
# Compute error
error = errorInSE3(M, Mdes)
v_error = v_frame - gMl.actInv(v_des)
# ~ a_corriolis = self.robot.acceleration(q,v,0*v,self._link_id, update_geometry)
# ~ a_corriolis.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T
# ~ a_tot = a_ref - gMl.actInv(a_corriolis)
return error, v_error.vector()
def errorLinkInSE3dyn(linkId, Mdes, v_des, q, v):
# Get the current configuration of the link
M = self.robot.position(q, linkId)
gMl = se3.SE3.Identity()
gMl.rotation = M.rotation
v_frame = self.robot.velocity(q, v, linkId)
# Compute error
error = errorInSE3(M, Mdes)
v_error = v_frame - gMl.actInv(v_des)
a_corriolis = self.robot.acceleration(q, v, 0 * v, linkId)
# ~ a_corriolis.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T
# ~ a_tot = gMl.actInv(a_corriolis) #a_ref - gMl.actInv(a_corriolis)
a_tot = a_corriolis
# ~ dJdq = a_tot.vector() *self.dt
dJdq = a_corriolis.vector()
return error, v_error.vector(), dJdq
def null(A, eps=1e-6): # -12
"""Compute a base of the null space of A."""
u, s, vh = np.linalg.svd(A)
padding = max(0, np.shape(A)[1] - np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0)
null_space = scipy.compress(null_mask, vh, axis=0)
return scipy.transpose(null_space)
XYZ_LF = np.array(Lf) + np.array([0.0, 0.0, 0.07])
RPY_LF = np.matrix([[0.0], [0.0], [0.0]])
SE3_LF = se3.SE3(se3.utils.rpyToMatrix(RPY_LF), XYZ_LF)
XYZ_RF = np.array(Rf) + np.array([0.0, 0.0, 0.07])
RPY_RF = np.matrix([[0.0], [0.0], [0.0]])
SE3_RF = se3.SE3(se3.utils.rpyToMatrix(RPY_RF), XYZ_RF)
# _RF________________________________________________________________
Jrf = self.robot.Jrf(self.q).copy()
Jrf[:3] = self.robot.Mrf(self.q).rotation * Jrf[:3, :] # Orient in the world base
v_ref = se3.se3.Motion(np.matrix([dRf[0], dRf[1], dRf[2], 0.0, 0.0, 0.0]).T)
errRf, v_errRf, dJdqRf = errorLinkInSE3dyn(self.robot.rf, SE3_RF, v_ref, self.q, self.v)
# _LF________________________________________________________________
Jlf = self.robot.Jlf(self.q).copy()
Jlf[:3] = self.robot.Mlf(self.q).rotation * Jlf[:3, :] # Orient in the world base
v_ref = se3.se3.Motion(np.matrix([dLf[0], dLf[1], dLf[2], 0.0, 0.0, 0.0]).T)
errLf, v_errLf, dJdqLf = errorLinkInSE3dyn(self.robot.lf, SE3_LF, v_ref, self.q, self.v)
# _COM_______________________________________________________________
Jcom = self.robot.Jcom(self.q)
p_com, v_com, a_com = self.robot.com(self.q, self.v, self.v * 0.0)
errCOM = self.robot.com(self.q) - (np.matrix(Com).T)
# ~ v_com = Jcom*self.v
v_errCOM = v_com - (np.matrix(dCom).T)
dJdqCOM = a_com
# _Trunk_____________________________________________________________
idx_Trunk = self.robot.index("root")
MTrunk0 = self.robot.position(self.robot.q0, idx_Trunk)
MTrunk = self.robot.position(self.q, idx_Trunk)
# errTrunk=errorInSE3(MTrunk0,MTrunk)[3:6]
JTrunk = self.robot.jacobian(self.q, idx_Trunk)[3:6]
# v_frame = self.robot.velocity(self.q,self.v,idx_Trunk)
# v_ref= se3.se3.Motion(np.matrix([.0,.0,.0,.0,.0,.0]).T)
# errTrunk,v_errTrunk = errorInSE3dyn(MTrunk,MTrunk0,v_frame,v_ref)
errTrunk, v_errTrunk, dJdqTrunk = errorLinkInSE3dyn(idx_Trunk, MTrunk0, v_ref, self.q, self.v)
errTrunk = errTrunk[3:6]
v_errTrunk = v_errTrunk[3:6]
dJdqTrunk = dJdqTrunk[3:6]
# _TASK1 STACK_______________________________________________________
K = 100.0
Kp_foot = K
Kp_com = K
Kp_Trunk = K
Kp_post = K
Kd_foot = 2 * np.sqrt(Kp_foot)
Kd_com = 2 * np.sqrt(Kp_com)
Kd_Trunk = 2 * np.sqrt(Kp_Trunk)
Kd_post = 2 * np.sqrt(Kp_post)
err1 = np.vstack([Kp_foot * errLf, Kp_foot * errRf, Kp_com * errCOM, Kp_Trunk * errTrunk])
v_err1 = np.vstack([Kd_foot * v_errLf, Kd_foot * v_errRf, Kd_com * v_errCOM, Kd_Trunk * v_errTrunk])
dJdq1 = np.vstack([dJdqLf, dJdqRf, dJdqCOM, dJdqTrunk])
J1 = np.vstack([Jlf, Jrf, Jcom, JTrunk])
# _TASK2 STACK_______________________________________________________
# _Posture___________________________________________________________
Jpost = np.hstack([zero([self.robot.nv - 6, 6]), eye(self.robot.nv - 6)])
errPost = Kp_post * (self.q - self.robot.q0)[7:]
v_errPost = Kd_post * (self.v - self.robot.v0)[6:]
errpost = -1 * (self.q - self.robot.q0)[7:]
err2 = errPost
v_err2 = v_errPost
J2 = Jpost
# Hierarchical solve_________________________________________________
qddot = npl.pinv(J1) * (-1.0 * err1 - 1.0 * v_err1 - 1.0 * dJdq1)
Z = null(J1)
qddot += Z * npl.pinv(J2 * Z) * (-(1.0 * err2 + 1.0 * v_err2) - J2 * qddot)
self.a = qddot
self.v += np.matrix(self.a * self.dt)
self.robot.increment(self.q, np.matrix(self.v * self.dt))
# ~ robotdoubleint(self.q,self.dq,qddot,self.dt)
# ~ self.v=self.dq/self.dt
self.robot.display(self.q)
self.robot.viewer.gui.refresh()
return self.robot.com(self.q), Jcom * self.v, errCOM, v_errCOM
# Tutorial 6
from pinocchio.romeo_wrapper import RomeoWrapper
robot = RomeoWrapper('/local/bchattop/devel/src/pinocchio/models/romeo.urdf')
import pinocchio as se3
from pinocchio.utils import *
import numpy as np
execfile("Script1.py")
q = robot.q0
v = rand(robot.nv)
pos_rw_des = np.mat('[0.5 ; 0.5 ; 0.5]')
z_la_des = robot.Mrh(robot.q0).translation[2,0]
# Altitude of right ankle
z_ra_des = robot.Mrh(robot.q0).translation[2,0]
for i in range(1000):
Mla = robot.Mrh(q)
e_la = Mla.translation[2,0] - z_la_des
J_la = Mla.rotation * robot.Jlf(q)[:3,:]
J_la = J_la[2,:]
#Right Foot/Ankle Jacobian
Mra = robot.Mrh(q)
e_ra = Mra.translation[2,0] - z_ra_des
J_ra = Mra.rotation * robot.Jrf(q)[:3,:]
J_ra = J_ra[2,:]
v=[0.2,.0]
#~ beta_x=3.0 #cost on pi-pi+1
#~ beta_y=6.0
#~ gamma=3.0
final_cost_on_p1= weighting_p0 *10.0
sigmaNoisePosition=0.00 #optional noise on COM measurement
sigmaNoiseVelocity=0.00
#initialisation of the pg
dt=durrationOfStep/pps
print( "dt= "+str(dt*1000)+"ms")
#load robot model
if (ROBOT_MODEL == "ROMEO"):
robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo.urdf")
#~ robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo_heavy_hand.urdf") #TO BE DEL
elif (ROBOT_MODEL == "REEMC"):
robot = ReemcWrapper("/home/tflayols/devel-src/reemc_wrapper/reemc/reemc.urdf")
robot.initDisplay()
robot.loadDisplayModel("world/pinocchio","pinocchio")
#Initial pose, stand on one feet (com = center of foot)
q_init=compute_initial_pose(robot)
#q_init=robot.q0.copy()
robot.display(q_init)
#pg = PgMini(Nstep,g,h,durrationOfStep,Dpy,beta_x,beta_y,gamma)
# Tutorial 4
from pinocchio.romeo_wrapper import RomeoWrapper
robot = RomeoWrapper('/local/bchattop/devel/src/pinocchio/models/romeo.urdf')
import pinocchio as se3
from pinocchio.utils import *
import numpy as np
execfile("Script1.py")
q = robot.q0
v = rand(robot.nv)
xdes = np.matrix([3.0,1.0,2.0]).T
for i in range(1000):
Mrh = robot.Mrh(q)
e = Mrh.translation[0:3,0] - xdes
J = Mrh.rotation * robot.Jrh(q)[:3,:]
qdot = -npl.pinv(J)*e
robot.increment(q,qdot*1e-2)
robot.display(q)
updateJointConfiguration(M,'l_wrist')
import pinocchio
from pinocchio.romeo_wrapper import RomeoWrapper
from pinocchio.utils import *
from pinocchio.explog import log
from pinocchio import SE3
import time
from numpy.linalg import inv, norm
from math import pi
from scipy.optimize import fmin_bfgs, fmin_slsqp
# --- 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
robot.viewer.gui.addSphere("world/yellow", .05,
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
import pinocchio as pin 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)
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)
class PinocchioController(object):
def __init__(self):
self.robot = RomeoWrapper("/local/tflayols/softwares/pinocchio/models/romeo.urdf")
self.robot.initDisplay()
self.robot.loadDisplayModel("world/pinocchio","pinocchio")
self.robot.display(self.robot.q0)
self.robot.viewer.gui.refresh()
self.q=np.copy(self.robot.q0)
def controlLfRfCom(self,Lf=[0.0,0.0,0.0],Rf=[0.0,0.0,0.0],Com=[0,0,0.63],K=1.0):
def robotint(q,dq):
M = se3.SE3( se3.Quaternion(q[6,0],q[3,0],q[4,0],q[5,0]).matrix(),q[:3])
dM = se3.exp(dq[:6])
M = M*dM
q[:3] = M.translation
q[3:7] = se3.Quaternion(M.rotation).coeffs()
q[7:] += dq[6:]
def errorInSE3( M,Mdes):
'''
Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
between M and Mdes, both element of SE3.
'''
error = se3.log(M.inverse()*Mdes)
return error.vector()
def null(A, eps=1e-12):
'''Compute a base of the null space of A.'''
u, s, vh = np.linalg.svd(A)
padding = max(0,np.shape(A)[1]-np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
null_space = scipy.compress(null_mask, vh, axis=0)
return scipy.transpose(null_space)
XYZ_LF=np.array(Lf)+np.array([.0,.0,0.07])
RPY_LF=np.matrix([[.0],[.0],[.0]])
SE3_LF=se3.SE3(se3.utils.rpyToMatrix(RPY_LF),XYZ_LF)
XYZ_RF=np.array(Rf)+np.array([.0,.0,0.07])
RPY_RF=np.matrix([[.0],[.0],[.0]])
SE3_RF=se3.SE3(se3.utils.rpyToMatrix(RPY_RF),XYZ_RF)
#_RF________________________________________________________________
Jlf=self.robot.Jlf(self.q).copy()
Jlf[:3] = self.robot.Mlf(self.q).rotation * Jlf[:3,:]#Orient in the world base
errRf = errorInSE3(SE3_RF,self.robot.Mrf(self.q))
#_LF________________________________________________________________
Jrf=self.robot.Jrf(self.q).copy()
Jrf[:3] = self.robot.Mrf(self.q).rotation * Jrf[:3,:]#Orient in the world base
errLf = errorInSE3(SE3_LF,self.robot.Mlf(self.q))
#_COM_______________________________________________________________
errCOM = self.robot.com(self.q)[:3]-(np.matrix(Com).T)[:3]
Jcom=self.robot.Jcom(self.q)[:3]
#_Trunk_____________________________________________________________
idx_Trunk = self.robot.index('root')
#~ ['universe',
#~ 'root',
#~ 'LHipYaw',
#~ 'LHipRoll',
#~ 'LHipPitch',
#~ 'LKneePitch',
#~ 'LAnklePitch',
#~ 'LAnkleRoll',
#~ 'RHipYaw',
#~ 'RHipRoll',
#~ 'RHipPitch',
#~ 'RKneePitch',
#~ 'RAnklePitch',
#~ 'RAnkleRoll',
#~ 'TrunkYaw',
#~ 'LShoulderPitch',
#~ 'LShoulderYaw',
#~ 'LElbowRoll',
#~ 'LElbowYaw',
#~ 'LWristRoll',
#~ 'LWristYaw',
#~ 'LWristPitch',
#~ 'NeckYaw',
#~ 'NeckPitch',
#~ 'HeadPitch',
#~ 'HeadRoll',
#~ 'RShoulderPitch',
#~ 'RShoulderYaw',
#~ 'RElbowRoll',
#~ 'RElbowYaw',
#~ 'RWristRoll',
#~ 'RWristYaw',
#~ 'RWristPitch']
#embed()
MTrunk0=self.robot.position(self.robot.q0,idx_Trunk)
MTrunk=self.robot.position(self.q,idx_Trunk)
errTrunk=errorInSE3(MTrunk0,MTrunk)[3:6]
JTrunk=self.robot.jacobian(self.q,idx_Trunk)[3:6]
#_TASK1 STACK_______________________________________________________
err1 = np.vstack([errLf,errRf,errCOM,errTrunk])
J1 = np.vstack([Jlf,Jrf,Jcom,JTrunk])
#_Posture___________________________________________________________
Jpost = np.hstack( [ zero([self.robot.nv-6,6]), eye(self.robot.nv-6) ] )
errpost = -1 * (self.q-self.robot.q0)[7:]
#embed()
#_TASK2 STACK_______________________________________________________
err2 = errpost
J2 = Jpost
#Hierarchical solve_________________________________________________
qdot = npl.pinv(J1)*-K * err1
Z = null(J1)
qdot += Z*npl.pinv(J2*Z)*(K*err2 - J2*qdot)
#__Integration______________________________________________________
robotint(self.q,qdot)
self.robot.display(self.q)
self.robot.viewer.gui.refresh()
return self.robot.com(self.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 --------------------------------------------------------------------
#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)
'''
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
def null(A, eps=1e-6):#-12
'''Compute a base of the null space of A.'''
u, s, vh = np.linalg.svd(A)
padding = max(0,np.shape(A)[1]-np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
null_space = scipy.compress(null_mask, vh, axis=0)
return scipy.transpose(null_space)
import time
print ("start")
LEFT_HAND_OBJECTIVE = 0
robot = RomeoWrapper("/home/flo/sources/pinocchio/models/romeo.urdf")
robot.initDisplay()
robot.loadDisplayModel("world/pinocchio","pinocchio")
q0 = robot.q0.copy()
v0 = robot.v0.copy()
v0ref = se3.se3.Motion(np.zeros(6))
q = robot.q0.copy()
v = robot.v0.copy()
a = robot.v0.copy()
robot.display(robot.q0)
# simulation timings
dt = 5e-3
final_time = 10.0
N = int(final_time/dt)
import numpy as np
from pinocchio.utils import *
import pinocchio as se3
from pinocchio.romeo_wrapper import RomeoWrapper
robot = RomeoWrapper('/home/bsinivas/devel/pinocchio/pinocchio/models/romeo.urdf')
#for name,function in robot.model.__class__.__dict__.items():
# print " **** ", name, ": ", function.__doc__
#def index(self,name):
# return [ idx for idx,name in enumerate(robot.model.names) if name=="RWristPitch" ][0]
# Get the placement of joint number idx
#placement = robot.data.oMi[0]
#q=zero(robot.nq)
#v=rand(robot.nv)
#robot.com(q) # Compute the robot center of mass.
#robot.position(q,23) # Compute the placement of joint 23
#lancer gepetto-viewer-server
#--------------creating and displaying the robot---------------
from gepetto.corbaserver import Client
client = Client ()
wid = client.gui.createWindow ("w")# open a new window
client.gui.createSceneWithFloor("world")
client.gui.addSceneToWindow("world",wid)
client.gui.addURDF("world/romeo","/home/bsinivas/devel/pinocchio/pinocchio/models/romeo.urdf","/home/bsinivas/devel/pinocchio/pinocchio/models/")
client.gui.getNodeList()
#-----------give a specific position to a joint-----------------
#q1 = [1,1,1,1,0,0,0]
#client.gui.applyConfiguration('world/romeo/HeadRollLink',q1)
#client.gui.applyConfiguration('world/romeo/LElbowYawLink',q1)
# Tutorials 1 and 2
from pinocchio.romeo_wrapper import RomeoWrapper
robot = RomeoWrapper('/local/bchattop/devel/src/pinocchio/models/romeo.urdf')
import pinocchio as se3
from pinocchio.utils import *
import numpy as np
robot.initDisplay("world/romeo/")
def updateJointConfiguration(M,name):
pinocchioConf = se3ToXYZQUAT(M)
viewerConf = XYZQUATToViewerConfiguration(pinocchioConf)
robot.viewer.gui.applyConfiguration('world/romeo/'+name,viewerConf)
robot.viewer.gui.refresh()
q = robot.q0
qdot = zero(robot.nv)
qdot[20] = 1
dt = 5e-3
for i in range(1000):
q[7:] += qdot[6:]*dt
robot.Mrh(q)
qdot[3] = 10
for i in range(100):
q[3] += qdot[3]*dt
robot.Mrh(q)
N = 1000
v = zero(3); v[2] = 1.0 / N
