def computeThroughFootWrenches(self, wrench_lf, wrench_rf): w_X_lf = self.robot.data.oMi[self.robot.legs['lf_foot']] w_X_rf = self.robot.data.oMi[self.robot.legs['rf_foot']] self.wrench_lf_W = w_X_lf.act(pin.Force(wrench_lf[:3], wrench_lf[3:])) self.wrench_rf_W = w_X_rf.act(pin.Force(wrench_rf[:3], wrench_rf[3:])) self.wrench_T = (self.wrench_lf_W + self.wrench_rf_W).vector return self.computeCoPFromWrench(self.wrench_T)
def _getContribution(self,com,s): ''' Get segment contribution to centroidal angular momenta and its rate of change Inputs: - s : segment index - com : position of the CoM in world reference frame ''' # get spatial inertia and velocity Y = self.robot.model.inertias[s] V = self.robot.data.v[s] # centroidal momenta in body frame # ihi = I Vi iHi = se3.Force(Y.matrix()*V.vector) # rate of change of centroidal momenta in body frame # ih_doti = Iai + Vi x* hi # TO VERIFY iHDi = (self.robot.model.inertias[s]*self.robot.data.a[s]) + se3.Inertia.vxiv(Y,V) #iHDi.linear # express at the center of mass oMc = se3.SE3.Identity() oMc.translation = self.robot.data.oMi[1].translation - com # uncomment in case need to change the rotation of the reference frame oMc.rotation = self.robot.data.oMi[1].rotation cMi = oMc.actInv( self.robot.data.oMi[s] ) cHi = cMi.act(iHi).np.A1 cHDi = cMi.act(iHDi).np.A1 return cHi, cHDi
def dyn_value(self, t, q, v): #(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t) vhg_ref = np.matrix([0., 0., 0., 0., 0., 0.]).T model = self.robot.model data = self.robot.data JMom = se3.ccrba(model, data, q, v) hg_prv = data.hg.vector.copy()[self._mask,:] #self.p_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:] #self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:] #self.v_error = self.robot.fext[self._mask,:] #self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:] #self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:] self.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:] #self.drift = 0 * self.a_des #self.a_des = #*********************** p_com = data.com[0] cXi = SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - p_com m_gravity = model.gravity.copy() model.gravity.setZero() b = se3.nle(model,data,q,v) model.gravity = m_gravity f_ff = se3.Force(b[:6]) f_com = cXi.act(f_ff) hg_drift = f_com.angular self.drift=f_com.np[self._mask,:] #************************ #print JMom.copy()[self._mask,:].shape #print self.__gain_matrix.shape self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix return self._jacobian, self.drift, self.a_des
def updateState(robot, q, v, sensor_data): # Get dcm from current state pin.forwardKinematics(robot.pinocchio_model_th, robot.pinocchio_data_th, q, v) comOut = pin.centerOfMass(robot.pinocchio_model_th, robot.pinocchio_data_th, q, v) vcomOut = robot.pinocchio_data_th.vcom[0] dcmOut = comOut + vcomOut / omega # Create zmp from forces forces = np.asarray(sensor_data[ForceSensor.type]) newWrench = pin.Force.Zero() for i, name in enumerate(contact_points): update_frame(robot.pinocchio_model_th, robot.pinocchio_data_th, name) placement = get_frame_placement(robot.pinocchio_model_th, robot.pinocchio_data_th, name) wrench = pin.Force(np.array([0.0, 0.0, forces[2, i]]), np.zeros(3)) newWrench += placement.act(wrench) totalWrenchOut = newWrench if totalWrenchOut.linear[2] > 0: zmpOut = [ -totalWrenchOut.angular[1] / totalWrenchOut.linear[2], totalWrenchOut.angular[0] / totalWrenchOut.linear[2] ] else: zmpOut = zmp_log return comOut, vcomOut, dcmOut, zmpOut, totalWrenchOut
def f12TOphi(f12, points=contact_Point): phi = pin.Force.Zero() for i in range(4): phii = pin.Force(f12[i * 3:i * 3 + 3], np.zeros(3)) fMi = pin.SE3(np.eye(3), points[:, i]) phi += fMi.act(phii) return phi
def dyn_value(self, t, q, v, update_geometry=False): g = self.robot.bias(q, 0 * v) b = self.robot.bias(q, v) b -= g M = self.robot.mass(q) com_p = self.robot.com(q) cXi = SE3.Identity() oXi = self.robot.data.oMi[1] cXi.rotation = oXi.rotation cXi.translation = oXi.translation - com_p b_com = cXi.actInv(se3.Force(b[:6, 0])) # b_com = cXi.actInv(b[:6,0]).vector b_com = b_com.angular # M_com = cXi.inverse().action.T * M[:6,:] # M_com = cXi.inverse().np.T * M[:6,:] M_com = self.robot.momentumJacobian(q, v, update_geometry) M_com = M_com[3:, :] L = M_com * v L_ref, dL_ref, ddL_ref = self._ref_traj(t) # acc = dL_ref - b_com[3:,:] dL_des = dL_ref - self.kp * (L - L_ref) return M_com[self._mask, :], b_com[self._mask, :], dL_des[self._mask, 0]
def computeWrench(cs, Robot, dt): rp = RosPack() urdf = rp.get_path( Robot.packageName ) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf' #srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf' robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) model = robot.model data = robot.data q_t = cs.concatenateQtrajectories() dq_t = cs.concatenateQtrajectories() ddq_t = cs.concatenateQtrajectories() t = q_t.min() for phase in cs.contactPhases: phase.wrench_t = None t = phase.timeInitial while t <= phase.timeFinal: pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t)) pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t), phase.ddq_t(t)) # FIXME : why do I need to call com to have the correct values in data ?? phi0 = data.oMi[1].act(pin.Force(data.tau[:6])) if phase.wrench_t is None: phase.wrench_t = piecewise( polynomial(phi0.vector.reshape(-1, 1), t, t)) else: phase.wrench_t.append(phi0.vector, t) t += dt if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.: t = phase.timeFinal
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): f = pin.Force.Random() f_array = np.array(f) f_from_array = pin.Force(f_array) self.assertTrue(f_from_array == f)
def step(self, q, vq, tauq, dt=None): if dt is None: dt = self.dt robot = self.robot NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK # --- Simu se3.forwardKinematics(robot.model, robot.data, q, v) se3.framesKinematics(robot.model, robot.data) Mrf = self.Mrf0.inverse() * robot.data.oMf[RF] vrf = robot.model.frames[RF].placement.inverse() * robot.data.v[RK] Mlf = self.Mlf0.inverse() * robot.data.oMf[LF] vlf = robot.model.frames[LF].placement.inverse() * robot.data.v[LK] qrf = np.vstack( [Mrf.translation[1:], se3.rpy.matrixToRpy(Mrf.rotation)[0]]) qlf = np.vstack( [Mlf.translation[1:], se3.rpy.matrixToRpy(Mlf.rotation)[0]]) frf = self.frf # Buffer where to store right force frf[[1, 2, 3]] = self.Krf * qrf # Right force in effector frame rf0_frf = se3.Force(frf) # Force in rf0 frame rk_frf = (robot.data.oMi[RK].inverse() * self.Mrf0).act( rf0_frf) # Spring force in R-knee frame. flf = self.flf # Buffer where to store left force flf[[1, 2, 3]] = self.Klf * qlf # Left force in effector frame lf0_flf = se3.Force(flf) # Force in lf0 frame lk_flf = (robot.data.oMi[LK].inverse() * self.Mlf0).act( lf0_flf) # Spring force in L-knee frame. self.forces = ForceDict({ RK: rk_frf, LK: lk_flf }, NB) # Argument to ABA aq = se3.aba(robot.model, robot.data, q, vq, tauq, self.forces) vq += aq * dt q = se3.integrate(robot.model, q, vq * dt) self.aq = aq return q, vq
def test_se3_action(self): f = se3.Force.Random() m = se3.SE3.Random() self.assertTrue( np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector)) self.assertTrue( np.allclose((m.actInv(f)).vector, m.action.T * f.vector)) v = se3.Motion.Random() f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]])) self.assertTrue(np.allclose((v**f).vector, zero(6)))
def test_force(self): m = self.m self.assertApprox(pin.Force.Zero().vector, zero(6)) f = pin.Force.Random() ff = f.linear ft = f.angular self.assertApprox(f.vector, np.concatenate([ff, ft])) self.assertApprox((m * f).vector, npl.inv(m.action.T).dot(f.vector)) self.assertApprox((m.actInv(f)).vector, m.action.T.dot(f.vector)) v = pin.Motion.Random() f = pin.Force(np.concatenate([v.vector[3:], v.vector[:3]])) self.assertApprox((v ^ f).vector, zero(6))
def test_force(self): m = self.m self.assertApprox(se3.Force.Zero().vector, zero(6)) f = se3.Force.Random() ff = f.linear ft = f.angular self.assertApprox(f.vector, np.vstack([ff, ft])) self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector) self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector) v = se3.Motion.Random() f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]])) self.assertApprox((v ^ f).vector, zero(6))
def setForces(self, data, forcesArr, forcesVec=None): ''' Convert a numpy array of forces into a stdVector of spatial forces. Side effect: keep the force values in data. ''' # In the dynamic equation, we wrote M*a + J.T*fdyn, while in the ABA it would be # M*a + b = tau + J.T faba, so faba = -fdyn (note the minus operator before a2m). data.f = forcesArr if forcesVec is None: forcesVec = data.forces data.forces[data.joint] *= 0 forcesVec[data.joint] += data.jMf * pinocchio.Force(a2m(forcesArr)) return forcesVec
def constraint_dyn(self, x, nc=0): ''' M aq + b(q,vq) + g(q) = tau_q + J(q)^T f = rnea(q,vq=0,aq=0,fs) ''' q, tauq, fr, fl = self.x2qf(x) # Forces should be stored in RNEA-compatible structure forces = pinocchio.StdVect_Force() for i in range(self.rmodel.njoints): forces.append(pinocchio.Force.Zero()) # Forces should be expressed at the joint for RNEA, while we store them # at the frame in the optim problem. Convert. jMr = rmodel.frames[self.idR].placement forces[self.jidR] = jMr * pinocchio.Force(fr) jMl = rmodel.frames[self.idL].placement forces[self.jidL] = jMl * pinocchio.Force(fl) #q = self.rmodel.neutralConfiguration aq = vq = zero(self.rmodel.nv) tauref = pinocchio.rnea(self.rmodel, self.rdata, q, vq, aq, forces) self.eq[nc:nc + self.rmodel.nv] = (tauref - tauq).flat return self.eq[nc:nc + self.rmodel.nv].tolist()
def computeThroughFootCoPs(self, wrench_lf, wrench_rf): self.wrench_LF = wrench_lf self.wrench_RF = wrench_rf cop_lf = self.computeCoPFromWrench(self.wrench_LF) cop_rf = self.computeCoPFromWrench(self.wrench_RF) # Expressing the local CoP position into the world frame w_X_lf = self.robot.data.oMi[self.robot.legs['lf_foot']] w_X_rf = self.robot.data.oMi[self.robot.legs['rf_foot']] cop_lf_W = w_X_lf * cop_lf cop_rf_W = w_X_rf * cop_rf self.wrench_LF_W = w_X_lf.act(pin.Force(wrench_lf[:3], wrench_lf[3:])).vector self.wrench_RF_W = w_X_lf.act(pin.Force(wrench_rf[:3], wrench_rf[3:])).vector # Computing the CoP of the system, i.e. # p = (f^lf_z * p^lf + f^rf_z * p^rf) / (f^lf_z + f^rf_z) fz_lf = np.asscalar(self.wrench_LF_W[2]) fz_rf = np.asscalar(self.wrench_RF_W[2]) self.cop = (fz_lf * cop_lf_W + fz_rf * cop_rf_W) / (fz_lf + fz_rf) return self.cop
def computeCoPFromWrench(self, wrench): cop = np.matrix([0., 0., 0.]).T # self.FOOT_FORCE_SENSOR_XYZ = np.matrix([0.0, 0.0, -0.085]).T # sole_X_ftSens = pin.SE3(np.eye(3), -self.FOOT_FORCE_SENSOR_XYZ); sole_X_ftSens = pin.SE3(np.eye(3), np.matrix([0., 0., 0.]).T) wrench = pin.Force(wrench[:3], wrench[3:]) wrench = sole_X_ftSens.act(wrench) # The CoP is defined as the point where the pressure force moments # vanishes. This occurs when tau = [p]x f, which the 6d force (i.e. # wrench) is [f,tau]. It also equivalents to: p = [n]x tau / (f.n), # which simplifies is p = [-tau_y, tau_x] / f_z cop[0] = -wrench.angular[1] / wrench.linear[2] cop[1] = wrench.angular[0] / wrench.linear[2] return cop
def computeWrench(res): rp = RosPack() urdf = rp.get_path( cfg.Robot.packageName ) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' #srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf' robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) model = robot.model data = robot.data for k in range(res.N): pin.rnea(model, data, res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k]) pcom, vcom, acom = robot.com( res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k] ) # FIXME : why do I need to call com to have the correct values in data ?? phi0 = data.oMi[1].act(pin.Force(data.tau[:6])) res.wrench_t[:, k] = phi0.vector return res
def _getBiais(self,q,v): model = self.robot.model data = self.robot.data p_com = self.robot.com(q) oXi = data.oMi[1] cXi = se3.SE3.Identity() cXi.rotation = oXi.rotation cXi.translation = oXi.translation - p_com #cXi.translation = p_com m_gravity = model.gravity.copy() model.gravity.setZero() b = se3.nle(model,data,q,v) model.gravity = m_gravity f_ff = se3.Force(b[:6]) f_com = cXi.act(f_ff) return f_com.np
def TransformForcesToJointLocalFrames(self, contact_forces, plate_contact_force_map, contact_force_joint_map, save_result=False): ''' Express each contact force w.r.t. the "local frame" of the contact point, i.e. the coordinate frame whose origin overlaps the point of contact and whose axis are in parallel with the world frame. ''' self.contact_force_joint_map = contact_force_joint_map oM_forceplates = self.ComputeForceplateDisplacements() contact_forces_local = np.zeros( (self.num_frames_in_video, self.num_contact_forces, 6)) for k in range(self.num_contact_forces): # Get contact joint id and force plate id using contact force id joint_id = contact_force_joint_map[k] forceplate_ids = [] for fpid, fid in enumerate(plate_contact_force_map): # fpid: force platform id # fid: contact force id if fid == k + 1: forceplate_ids.append(fpid) if len(forceplate_ids) == 0: continue for i in range(self.num_frames_in_video): p_c = self.joint_3d_positions[i, joint_id, :] # in meters force_wrt_c = se3.Force(zero(6)) for fpid in forceplate_ids: # Compute forceplate displacement wrt person joint cR_plate = oM_forceplates[fpid].rotation.copy() p_plate = oM_forceplates[fpid].translation.copy() if fpid in [0, 1]: # force plates on the floor relative_position = p_plate - np.matrix(p_c).T cM_plate = se3.SE3(cR_plate, relative_position) elif fpid in [2, 3]: # force sensors on the shelf cM_plate = se3.SE3(cR_plate, zero(3)) else: raise ValueError("Should never happen!") # Express the 6D force k in person joint's local frame force_wrt_plate = se3.Force( np.matrix(self.contact_forces[i, fpid, :]).T) force_wrt_c += cM_plate.act(force_wrt_plate) contact_forces_local[i, k, :] = \ force_wrt_c.vector.getA().reshape(-1) self.contact_forces_local = contact_forces_local # save the results to file if save_result: results = { "contact_forces_local": self.contact_forces_local, "contact_forces_names": self.contact_force_names, "fps": self.fps_video } if hasattr(self, "mask_uncaptured_forces"): results["mask_uncaptured_forces"] = \ self.mask_uncaptured_forces save_path = join(self.parkour_dataset, "gt_motion_forces", "contact_forces_local.pkl") check_and_save_data(results, save_path, self.video_name) print(' Contact force saved to {}'.format(save_path))
b = pinocchio.rnea(model, data, q, v, zero(model.nv)).copy() M = pinocchio.crba(model, data, q).copy() # ahat = a.l + wxv pinocchio.forwardKinematics(model, data, q, v, zero(model.nv)) gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear) # M a + b = tau + J'f # J a = -gamma K = np.bmat([[M, J.T], [J, zero([3, 3])]]) r = np.concatenate([tau - b, -gamma]) af = inv(K) * r a = af[:model.nv] f = af[model.nv:] fs[-1] = pinocchio.Force(-f, zero(3)) assert (absmax(rnea(model, data, q, v, a, fs) - (M * a + b + J.T * f)) < 1e-6) assert ( absmax(aba(model, data, q, v, tau, fs) - (inv(M) * (tau - b - J.T * f))) < 1e-6) # Check contact-dyninv deriv # af = Ki r = [ MJt; J0 ] [ tau-b;-gamma ] # af' = -Ki K' Ki r + Ki r' # = -Ki ( K' af - r' ) # = -Ki [ M'a + Jt'f + b' ; J' a + gamma' ] # Check M'a + J'f + b' dtau_dqn = df_dq(model, lambda _q: rnea(model, data, _q, v, a, fs), q) dtau_dvn = df_dq(model, lambda _v: rnea(model, data, q, _v, a, fs), v)
print( 'NOTE: "predicted" wrench values are not accurate due to the foot saturation and as such they are not checked.' ) print( "CoP values are predicted under the assumption that they are at the foot border and as such they are checked." ) # --- Wrench saturation (left) --- print() print("--- Wrench saturation (left) ---") distribute.phase.value = 1 distribute.phase.time = 1 wrenchLeft = wrench ankleWrenchLeft = list( leftPos.actInv(pin.Force(np.matrix(wrenchLeft).T)).vector.flat) print("expected global wrench: %s" % str(wrench)) print("expected global left wrench: %s" % str(wrenchLeft)) print("expected ankle left wrench: %s" % str(ankleWrenchLeft)) copLeft = [ float(com[0] - leftPos.translation[0]), distribute_conf.RIGHT_FOOT_SIZES[3], 0. ] print("expected sole left CoP: %s" % str(copLeft)) print() distribute.zmpRef.recompute(1)
def computeResultTrajectory(robot, t_t, q_t, v_t, a_t): model = robot.model data = robot.data N = q_t.shape[1] ZMP_t = np.matrix(np.zeros([3, N])) waist_t = np.matrix(np.zeros([3, N])) pcom_t = np.matrix(np.empty([3, N])) vcom_t = np.matrix(np.empty([3, N])) acom_t = np.matrix(np.empty([3, N])) tau_t = np.matrix(np.empty([robot.nv, N])) wrench_t = np.matrix(np.empty([6, N])) waist_orientation_t = np.matrix(np.empty([3, N])) # Sample end effector traj ee_t = dict() RF_t = [] ee_t["rf"] = (RF_t) LF_t = [] ee_t["lf"] = (LF_t) RH_t = [] ee_t["rh"] = (RH_t) LH_t = [] ee_t["lh"] = (LH_t) Mee = dict() Mee["rf"] = robot.Mrf Mee["lf"] = robot.Mlf Mee["rh"] = robot.Mrh Mee["lh"] = robot.Mlh ee_names = list(ee_t.viewkeys()) for k in range(N): q = q_t[:, k] v = v_t[:, k] a = a_t[:, k] #M = robot.mass(q) #b = robot.biais(q,v) #robot.dynamics(q,v,0*v) se3.rnea(model, data, q, v, a) #se3.forwardKinematics(model,data,q) #robot.computeJacobians(q) pcom, vcom, acom = robot.com(q, v, a) # Update EE placements for ee in ee_names: ee_t[ee].append(Mee[ee](q, update_kinematics=False).copy()) # Update CoM data pcom_t[:, k] = pcom vcom_t[:, k] = vcom acom_t[:, k] = acom #oXi_s = robot.data.oMi[1].inverse().np.T #phi0 = oXi_s * (M[:6,:] * a + b[:6]) tau_t[:, k] = data.tau phi0 = data.oMi[1].act(se3.Force(data.tau[:6])) wrench_t[:, k] = phi0.vector forces = wrench_t[:3, k] torques = wrench_t[3:, k] ZMP_t[0, k] = -torques[1] / forces[2] ZMP_t[1, k] = torques[0] / forces[2] waist_t[:, k] = robot.data.oMi[1].translation waist_orientation_t[:, k] = matrixToRpy(robot.data.oMi[1].rotation) result = Struct() result.t_t = t_t result.ZMP_t = ZMP_t result.waist_t = waist_t result.waist_orientation_t = waist_orientation_t result.pcom_t = pcom_t result.vcom_t = vcom_t result.acom_t = acom_t result.wrench_t = wrench_t result.tau_t = tau_t result.q_t = q_t result.v_t = v_t result.a_t = a_t result.ee_t = ee_t return result
stop = distribute.emergencyStop.value np.testing.assert_equal(stop, 0) # --- Wrench saturation (left) --- print() print("--- Wrench saturation ---") print('NOTE: no actual "saturation" is performed. As such, the resulting CoP may be outside the support area') # --- Wrench saturation (left) --- print() print("--- Wrench saturation (left) ---") distribute.phase.value = 1 distribute.phase.time = 1 wrenchLeft = wrench ankleWrenchLeft = list(leftPos.actInv(pin.Force(np.matrix(wrenchLeft).T)).vector.flat) print("expected global wrench: %s" % str(wrench)) print("expected global left wrench: %s" % str(wrenchLeft)) print("expected ankle left wrench: %s" % str(ankleWrenchLeft)) copLeft = [float(com[0] - leftPos.translation[0]), float(com[1] - leftPos.translation[1]), 0.] print("expected sole left CoP: %s" % str(copLeft)) print() distribute.zmpRef.recompute(1) print("resulting global wrench: %s" % str(distribute.wrenchRef.value)) assertApprox(wrench, distribute.wrenchRef.value, 2) print("resulting global left wrench: %s" % str(distribute.wrenchLeft.value))
Jc = pio.jacobianCenterOfMass(rm, rd, q) w_T_b = rd.oMi[1] # world to body transform # Check that the Inertia matrix expressed at the center of mass is of the form: # c_I_c = b_Mc[:6,:6] = [[m*Id3 03 ] # [03 I(q)]] pio.centerOfMass(rm, rd, q, vq) c_T_b = pio.SE3(w_T_b.rotation, -rd.com[0] + w_T_b.translation) # Ic = cXb^star * Ib * bXc c_I_c = c_T_b.actionInverse.T @ b_I_b @ c_T_b.actionInverse # momentum coordinate transform assert (np.allclose(c_I_c[:3, :3], rd.mass[0] * np.eye(3))) # Check that M[:6,:] is the centroidal momentum expressed in F_b c_hc = pio.computeCentroidalMomentum(rm, rd, q, vq) b_hc = pio.Force(b_Mc[:6, :] @ vq) assert ((c_hc - c_T_b * b_hc).isZero()) # Check that the linear momentum is m*cd pio.centerOfMass(rm, rd, q, vq) cd = rd.vcom[0] m = rd.mass[0] np.allclose(c_hc.linear, m * cd) # Transforms vq to "centroid vel configuration vect" in universe frame Z = np.vstack([ Jc, np.hstack([np.zeros([3, 3]), w_T_b.rotation, np.zeros([3, rm.nv - 6])]), np.hstack([np.zeros([rm.nv - 6, 6]), np.eye(rm.nv - 6)])
cy = [] for d in data: q = config_sot_to_urdf(np.asmatrix(d.q)) wrench_lf = d.f_lf wrench_rf = d.f_rf # Evaluation of the center of pressure from different methods hrp2.update(q) p = cop.compute(wrench_lf, wrench_rf, 'FootWrenches') c = pin.centerOfMass(hrp2.model, hrp2.data, q) print 'foot wrenches', cop.compute(wrench_lf, wrench_rf, 'FootWrenches').T print 'foot cops ', cop.compute(wrench_lf, wrench_rf, 'FootCoPs').T wrench_lf_s = hrp2.compute_sMa(wrench_lf).act( pin.Force(wrench_lf[:3], wrench_lf[3:])).vector wrench_rf_s = hrp2.compute_sMa(wrench_rf).act( pin.Force(wrench_rf[:3], wrench_rf[3:])).vector wrench_t = wrench_lf_s + wrench_rf_s mass = np.linalg.norm(wrench_t[:3]) / 9.81 print 'mass ', mass print '---' cx.append(np.asscalar(c[0])) cy.append(np.asscalar(c[1])) px.append(np.asscalar(p[0])) py.append(np.asscalar(p[1])) #import pinocchio as pin #print pin.centerOfMass(hrp2.model, hrp2.data, q).T x = np.arange(-0.02, 0.02, 0.001)
assert(isapprox(m.actInv(p),npl.inv(m.action())*p)) # --- Motion assert(isapprox(se3.Motion.Zero().vector(),zero(6))) v = se3.Motion.Random() assert(isapprox((m*v).vector(),m.action()*v.vector())) assert(isapprox((m.actInv(v)).vector(),npl.inv(m.action())*v.vector())) vv = v.linear; vw = v.angular assert(isapprox( v.vector(),np.vstack([vv,vw]) )) assert(isapprox((v**v).vector(),zero(6))) # --- Force --- assert(isapprox(se3.Force.Zero().vector(),zero(6))) f = se3.Force.Random() ff = f.linear; ft = f.angular assert(isapprox( f.vector(),np.vstack([ff,ft]) )) assert(isapprox((m*f).vector(),npl.inv(m.action().T)*f.vector())) assert(isapprox((m.actInv(f)).vector(),m.action().T*f.vector())) f = se3.Force( np.vstack([ v.vector()[3:], v.vector()[:3] ]) ) assert(isapprox( (v**f).vector(),zero(6) )) # --- Inertia --- Y1 = se3.Inertia.Random() Y2 = se3.Inertia.Random() Y=Y1+Y2 assert(isapprox(Y1.matrix()+Y2.matrix(),Y.matrix())) assert(isapprox((Y*v).vector(),Y.matrix()*v.vector())) assert(isapprox( (m*Y).matrix(),m.inverse().action().T*Y.matrix()*m.inverse().action())) assert(isapprox( (m.actInv(Y)).matrix(),m.action().T*Y.matrix()*m.action()))
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') robot = RobotWrapper(URDF, [PKG])