Example #1
0
#q1 = [1,1,1,1,0,0,0]
#client.gui.applyConfiguration('world/romeo/HeadRollLink',q1)
#client.gui.applyConfiguration('world/romeo/LElbowYawLink',q1)
#client.gui.applyConfiguration('world/romeo/LHipPitchLink',q1)
#client.gui.applyConfiguration('world/romeo/LKneePitchLink',q1)
#client.gui.applyConfiguration('world/romeo/LShoulderYawLink',q1)
#client.gui.applyConfiguration('world/romeo/LWristRollLink',q1)
#client.gui.applyConfiguration('world/romeo/LWristYawLink',q1)
#client.gui.applyConfiguration('world/romeo/NeckPitchLink',q1)
#client.gui.applyConfiguration('world/romeo/RElbowYawLink',q1)
#client.gui.applyConfiguration('world/romeo/RHipPitchLink',q1)
#client.gui.applyConfiguration('world/romeo/RKneePitchLink',q1)
#client.gui.applyConfiguration('world/romeo/RShoulderYawLink',q1)
#client.gui.applyConfiguration('world/romeo/RWristRollLink',q1)
#client.gui.applyConfiguration('world/romeo/RWristYawLink',q1)
#client.gui.applyConfiguration('world/romeo/body',q1)
#client.gui.applyConfiguration('world/romeo/l_ankle',q1)
#client.gui.applyConfiguration('world/romeo/l_wrist',q1)
#client.gui.applyConfiguration('world/romeo/r_ankle',q1)
#client.gui.applyConfiguration('world/romeo/r_wrist',q1)
#client.gui.applyConfiguration('world/romeo/torso',q1)
#client.gui.refresh() # Refresh the window.

#head_joint_id=robot.index('HeadRoll')
#M=robot.position(robot.q0,head_joint_id)

robot.initDisplay("world/romeo") #init the displaying of the robot
robot.display(robot.q0) # display the robot at the initial position
q=robot.q0
v = rand(robot.nv)
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)  



MPC = MPC_Formulation_DS(Nstep, 
                         g,  
                         h,  
Example #3
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
robot.viewer.gui.addSphere("world/yellow", .05,
                           [1., .2, 1., .5])  # .1 is the radius
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)
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)
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

  cl = gepetto.corbaserver.Client()
  gui = cl.gui
  if gui.nodeExists("world"):
    gui.deleteNode("world","ON")

  robot.initDisplay(loadModel=False)
  robot.loadDisplayModel("pinocchio")
  robot.display(robot.q0)
  gui = robot.viewer.gui
Example #7
0
#-----------------------------------------------------------------------------
#---- 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 --------------------------------------------------------------------
#-----------------------------------------------------------------------------
#pinocchioModel = pin.buildModelFromUrdf(urdfpath, pin.JointModelFreeFlyer())
Example #8
0
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,
                           [1., .2, 1., .5])  # .1 is the radius
robot.viewer.gui.addSphere("world/white", .05,
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
Example #10
0
# 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