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()