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