def __init__(self, name, robot, frameName):
TaskMotion.__init__(self, name, robot)
self.frameName = frameName
self.constraint = ConstraintEquality(name, rows=6, cols=robot.nv)
self.ref = TrajectorySample(12, 6)
assert self.robot.model.existFrame(frameName)
self.id = self.robot.model.getFrameId(frameName)
self.v_ref = se3.Motion()
self.a_Ref = se3.Motion()
self.M_ref = se3.SE3()
self.wMl = se3.SE3()
self.p_error_vec = np.zeros(6)
self.v_error_vec = np.zeros(6)
self.p = np.zeros(12)
self.v = np.zeros(6)
self.p_ref = np.zeros(12)
self.v_ref_vec = np.zeros(6)
self.Kp = np.zeros(6)
self.Kd = np.zeros(6)
self.a_des = np.zeros(6)
self.J = np.zeros((6, self.robot.nv))
self.J_rotated = np.zeros((6, self.robot.nv))
self.mask = np.ones(6)
self.setMask(self.mask)
self.local = True
def writeFromMessage(self, msg):
t = msg.time
q = np.zeros(self._model.nq)
v = np.zeros(self._model.nv)
tau = np.zeros(self._model.njoints - 2)
p = dict()
pd = dict()
f = dict()
s = dict()
# Retrieve the generalized position and velocity, and joint torques
q[3] = msg.centroidal.base_orientation.x
q[4] = msg.centroidal.base_orientation.y
q[5] = msg.centroidal.base_orientation.z
q[6] = msg.centroidal.base_orientation.w
v[3] = msg.centroidal.base_angular_velocity.x
v[4] = msg.centroidal.base_angular_velocity.y
v[5] = msg.centroidal.base_angular_velocity.z
for j in range(len(msg.joints)):
jointId = self._model.getJointId(msg.joints[j].name) - 2
q[jointId + 7] = msg.joints[j].position
v[jointId + 6] = msg.joints[j].velocity
tau[jointId] = msg.joints[j].effort
pinocchio.centerOfMass(self._model, self._data, q, v)
q[0] = msg.centroidal.com_position.x - self._data.com[0][0]
q[1] = msg.centroidal.com_position.y - self._data.com[0][1]
q[2] = msg.centroidal.com_position.z - self._data.com[0][2]
v[0] = msg.centroidal.com_velocity.x - self._data.vcom[0][0]
v[1] = msg.centroidal.com_velocity.y - self._data.vcom[0][1]
v[2] = msg.centroidal.com_velocity.z - self._data.vcom[0][2]
# Retrive the contact information
for contact in msg.contacts:
name = contact.name
# Contact pose
pose = contact.pose
position = np.array(
[pose.position.x, pose.position.y, pose.position.z])
quaternion = pinocchio.Quaternion(pose.orientation.w,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z)
p[name] = pinocchio.SE3(quaternion, position)
# Contact velocity
velocity = contact.velocity
lin_vel = np.array(
[velocity.linear.x, velocity.linear.y, velocity.linear.z])
ang_vel = np.array(
[velocity.angular.x, velocity.angular.y, velocity.angular.z])
pd[name] = pinocchio.Motion(lin_vel, ang_vel)
# Contact wrench
wrench = contact.wrench
force = np.array([wrench.force.x, wrench.force.y, wrench.force.z])
torque = np.array(
[wrench.torque.x, wrench.torque.y, wrench.torque.z])
f[name] = [contact.type, pinocchio.Force(force, torque)]
# Surface normal and friction coefficient
normal = contact.surface_normal
nsurf = np.array([normal.x, normal.y, normal.z])
s[name] = [nsurf, contact.friction_coefficient]
return t, q, v, tau, p, pd, f, s
def test_conversion(self):
m = pin.Motion.Random()
m_array = np.array(m)
m_from_array = pin.Motion(m_array)
self.assertTrue(m_from_array == m)
def test_se3_action(self):
f = pin.Force.Random()
m = pin.SE3.Random()
self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose(m.act(f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
v = pin.Motion(np.vstack([f.vector[3:], f.vector[:3]]))
self.assertTrue(np.allclose((v ^ f).vector, zero(6)))
def __init__(self, state, xref, gains=[0., 0.]):
crocoddyl.ContactModelAbstract.__init__(self, state, 3)
self.xref = xref
self.gains = gains
self.joint = state.pinocchio.frames[xref.frame].parent
self.fXj = state.pinocchio.frames[xref.frame].placement.inverse().action
v = pinocchio.Motion().Zero()
self.vw = v.angular
self.vv = v.linear
self.Jw = pinocchio.utils.zero((3, state.pinocchio.nv))
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = robot.q # actuation joint position
qa_dot = robot.v # actuation joint velocity
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(qa,
ID_EE) ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.vector
v_vec = v_error.vector
for i in range(0, 3): # for position only control
p_vec[i + 3] = 0.0
v_vec[i + 3] = 0.0
f_star = Lam * (-Kp * p_vec - 2.0 * np.sqrt(Kp) * v_vec)
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
return torque
import pinocchio as se3
from pinocchio.utils import *
norm = npl.norm
import time
N = 1000
M = se3.SE3.Identity()
# Integrate a constant body velocity.
v = zero(3)
v[2] = 1.0 / N
w = zero(3)
w[1] = 1.0 / N
nu = se3.Motion(v, w)
for i in range(N):
M = se3.exp(nu) * M
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
Mc = se3.SE3(M.rotation, zero(3))
M = M * se3.exp(Mc.actInv(nu))
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a constant "log" velocity in body frame.
ME = se3.SE3(
def estimate(transforms, Tm):
errs = [ pinocchio.log6(Tm.actInv(T)).vector for T in transforms ]
v_mean = np.mean(errs, 0)
v_var = np.var(errs, 0)
Tm = Tm * pinocchio.exp6(pinocchio.Motion(v_mean))
return Tm, v_var
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = np.matrix(np.zeros((7, 1)))
qa_dot = np.matrix(np.zeros((7, 1)))
for i in range(7):
qa[i, 0] = robot.q[i]
qa_dot[i, 0] = robot.v[i]
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(
qa, ID_EE)[:3, :] ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.linear
v_vec = v_error.linear
xddotstar = (-Kp * p_vec - 2 * np.sqrt(Kp) * v_vec)
f_star = Lam * xddotstar
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
# Selection Matrix
Id7 = np.identity(7)
fault_joint_num = 0
S = np.delete(Id7, (fault_joint_num),
axis=0) #delete fault joint corresponding row
# Selection Matrix Inverse - dynamically consistent inverse
Minv = np.linalg.inv(M)
ST = S.transpose()
SbarT = np.linalg.pinv(S * Minv * ST) * S * Minv
# Jacobian Matrix Inverse - dynamically consistent inverse
JbarT = Lam * J * np.linalg.inv(M)
#Weighting matrix
W = S * Minv * ST
Winv = np.linalg.inv(W)
#Weighted pseudo inverse of JbarT*ST
JtildeT = Winv * (JbarT * ST).transpose() * np.linalg.pinv(
JbarT * ST * Winv * (JbarT * ST).transpose())
#Null-space projection matrix
Id6 = np.identity(6)
NtildeT = Id6 - JtildeT * JbarT * ST
null_torque = 0.0 * qa_dot # joint damping
#Torque
torque = ST * (JtildeT * f_star + NtildeT * SbarT * null_torque +
SbarT * NLE)
return torque
#mprint(rand([6, 6])) # Matlab-style print
skew(rand(3)) # Skew "cross-product" 3x3 matrix from a 3x1 vector
cross(rand(3), rand(3)) # Cross product of R^3
rotate('x', 0.4) # Build a rotation matrix of 0.4rad around X.
import pinocchio as se3
R = eye(3)
p = zero(3)
M0 = se3.SE3(R, p)
M = se3.SE3.Random()
M.translation = p
M.rotation = R
v = zero(3)
w = zero(3)
nu0 = se3.Motion(v, w)
nu = se3.Motion.Random()
nu.linear = v
nu.angular = w
f = zero(3)
tau = zero(3)
phi0 = se3.Force(f, tau)
phi = se3.Force.Random()
phi.linear = f
phi.angular = tau
from pinocchio.robot_wrapper import RobotWrapper
from os.path import join
PKG = '/home/alumno04/dev_samir/ros/sawyer_ws/src/'
URDF = join(PKG, '/sawyer_robot/sawyer_description/urdf/model1.urdf')
def setReference(self, ref):
self.ref = copy.deepcopy(ref)
self.M_ref.translation = copy.deepcopy(ref.getPos()[:3])
self.M_ref.rotation = copy.deepcopy(ref.getPos()[3:].reshape((3, 3)))
self.v_ref = se3.Motion(ref.getVel())
self.a_ref = se3.Motion(ref.getAcc())
# [-s, 0, c]
# ])
# target = np.array([np.sin(angle), 0, np.cos(angle)]))
runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)
target = np.array(conf.target)
footName = 'foot'
footFrameID = robot_model.getFrameId(footName)
assert (robot_model.existFrame(footName))
Pref = crocoddyl.FrameTranslation(footFrameID, target)
# If also the orientation is useful for the task use
# Mref = crocoddyl.FramePlacement(footFrameID,
# pinocchio.SE3(R, conf.n_links * np.matrix([[np.sin(angle)], [0], [np.cos(angle)]])))
footTrackingCost = crocoddyl.CostModelFrameTranslation(state, Pref,
actuation.nu)
Vref = crocoddyl.FrameMotion(footFrameID, pinocchio.Motion(np.zeros(6)))
footFinalVelocity = crocoddyl.CostModelFrameVelocity(state, Vref, actuation.nu)
# simulating the cost on the power with a cost on the control
power_act = crocoddyl.ActivationModelQuad(conf.n_links)
u2 = crocoddyl.CostModelControl(
state, power_act,
actuation.nu) # joule dissipation cost without friction, for benchmarking
stateAct = crocoddyl.ActivationModelWeightedQuad(
np.concatenate([np.zeros(state.nq + 1),
np.ones(state.nv - 1)]))
v2 = crocoddyl.CostModelState(state, stateAct, np.zeros(state.nx),
actuation.nu)
joint_friction = CostModelJointFrictionSmooth(state, power_act, actuation.nu)
joule_dissipation = CostModelJouleDissipation(state, power_act, actuation.nu)
# PENALIZATIONS
b_a = np.random.random(3) - 0.5
b_w = np.random.random(3) - 0.5
b_proper_acc = b_a - oRb_int.T @ gravity
# direct preint
p_int = p_int + v_int * dt + 0.5 * oRb_int @ b_a * dt**2
v_int = v_int + oRb_int @ b_a * dt
oRb_int = oRb_int @ pin.exp(b_w * dt)
# preint
Deltat += dt
deltak = compute_current_delta_IMU(b_proper_acc, b_w, dt)
DeltaIMU = compose_delta_IMU(DeltaIMU, deltak, dt)
x_imu_int = state_plus_delta_IMU(x_imu_ori, DeltaIMU, Deltat)
# pin
b_nu_int += pin.Motion(b_dnu * dt)
import matplotlib.pyplot as plt
v_direct_arr = np.array(v_direct_lst)
v_preint_arr = np.array(v_preint_lst)
v_err = v_direct_arr - v_preint_arr
plt.figure('Velocity direct - preint (error)')
plt.plot(t_arr, v_err[:, 0], 'r')
plt.plot(t_arr, v_err[:, 1], 'g')
plt.plot(t_arr, v_err[:, 2], 'b')
plt.show()
