def test_getters(self): data = self.model.createData() q = pin.randomConfiguration(self.model) v = np.random.rand(self.model.nv) a = np.random.rand(self.model.nv) pin.forwardKinematics(self.model, data, q, v, a) T = pin.updateFramePlacement(self.model, data, self.frame_idx) self.assertApprox(T, data.oMi[self.parent_idx].act(self.frame_placement)) v = pin.getFrameVelocity(self.model, data, self.frame_idx) self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) self.assertApprox( v, data.oMi[self.parent_idx].act(data.v[self.parent_idx])) v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) self.assertApprox( v, pin.SE3(T.rotation, np.zeros(3)).act( self.frame_placement.actInv(data.v[self.parent_idx]))) a = pin.getFrameAcceleration(self.model, data, self.frame_idx) self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) self.assertApprox(a, self.frame_placement.actInv(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) self.assertApprox( a, data.oMi[self.parent_idx].act(data.a[self.parent_idx])) a = pin.getFrameAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) self.assertApprox( a, pin.SE3(T.rotation, np.zeros(3)).act( self.frame_placement.actInv(data.a[self.parent_idx]))) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL) a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD) a = pin.getFrameClassicalAcceleration( self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def get_body_world_acceleration(robot: jiminy.Model, body_name: str, use_theoretical_model: bool = True) -> pin.SE3: """Get the body spatial acceleration in world frame. The moment of this tensor (i.e linear part) is NOT the linear acceleration of the center of the body frame, expressed in the world frame. .. warning:: It is assumed that `update_quantities` has been called. :param robot: Jiminy robot. :param body_name: Name of the body. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the robot's state. Optional: True by default. :returns: Spatial acceleration. """ # Pick the right pinocchio model and data if use_theoretical_model: pnc_model = robot.pinocchio_model_th pnc_data = robot.pinocchio_data_th else: pnc_model = robot.pinocchio_model pnc_data = robot.pinocchio_data # Get frame index and make sure it exists body_id = pnc_model.getFrameId(body_name) assert body_id < pnc_model.nframes, f"Frame '{body_name}' does not exits." return pin.getFrameAcceleration( pnc_model, pnc_data, body_id, pin.LOCAL_WORLD_ALIGNED)
def calc(self, data, x): assert (self.Mref.oMf is not None or self.gains[0] == 0.) data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame, pinocchio.ReferenceFrame.LOCAL) data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector if self.gains[0] != 0.: self.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame] data.a0 += np.asscalar(self.gains[0]) * pinocchio.log6(self.rMf).vector if self.gains[1] != 0.: v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector data.a0 += np.asscalar(self.gains[1]) * v
def calc(self, data, x): assert (self.xref.oxf is not None or self.gains[0] == 0.) v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.xref.frame) self.vw = v.angular self.vv = v.linear fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL) data.Jc = fJf[:3, :] self.Jw = fJf[3:, :] data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.xref.frame).linear + pinocchio.utils.cross(self.vw, self.vv) if self.gains[0] != 0.: data.a0 += np.asscalar(self.gains[0]) * (data.pinocchio.oMf[self.xref.frame].translation - self.xref.oxf) if self.gains[1] != 0.: data.a0 += np.asscalar(self.gains[1]) * self.vv
def calc(self, data, x): # We suppose forwardKinematics(q,v,a), computeJointJacobian and updateFramePlacement already # computed. assert (self.ref is not None or self.gains[0] == 0.) data.J[:, :] = pinocchio.getFrameJacobian( self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL) data.a0[:] = pinocchio.getFrameAcceleration(self.pinocchio, data.pinocchio, self.frame).vector.flat if self.gains[0] != 0.: data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame] data.a0[:] += self.gains[0] * m2a(pinocchio.log(data.rMf).vector) if self.gains[1] != 0.: data.a0[:] += self.gains[1] * m2a( pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio, self.frame).vector)
def calc(self, data, x): # We suppose forwardKinematics(q,v,np.zero), computeJointJacobian and updateFramePlacement already # computed. assert (self.ref is not None or self.gains[0] == 0.) data.v = pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio, self.frame).copy() vw = data.v.angular vv = data.v.linear J6 = pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL) data.J[:, :] = J6[:3, :] data.Jw[:, :] = J6[3:, :] data.a0[:] = (pinocchio.getFrameAcceleration( self.pinocchio, data.pinocchio, self.frame).linear + cross(vw, vv)).flat if self.gains[0] != 0.: data.a0[:] += self.gains[0] * ( m2a(data.pinocchio.oMf[self.frame].translation) - self.ref) if self.gains[1] != 0.: data.a0[:] += self.gains[1] * m2a(vv)
M = data.M Mi = pinocchio.computeMinverse(model, data, q) da = data.ddq_dq dtau = data.dtau_dq assert (absmax(M * da + dtau) < 1e-9) assert (absmax(da + Mi * dtau) < 1e-9) pinocchio.forwardKinematics(model, data, q, v, a) pinocchio.updateFramePlacements(model, data) frameJacobian = pinocchio.getFrameJacobian(model, data, 10, pinocchio.ReferenceFrame.LOCAL) frameJacobianTimeVariation = pinocchio.getFrameJacobianTimeVariation( model, data, 10, pinocchio.ReferenceFrame.LOCAL) frameAcceleration = pinocchio.getFrameAcceleration(model, data, 10) assert (absmax(frameJacobian * a + frameJacobianTimeVariation * v - frameAcceleration.vector) < 1e-9) ''' (a,f) = K^-1 (tau-b,-gamma) avec K = [ M J* ; J 0 ] (a',f') = -K^-1 K' K^-1 (tau-b,-gamma) - K^-1 (b';gamma') = -Ki K' (a,f) - Ki (b';g') = -Ki ( K'(a,f) - (b',g') ) ''' # Define finite-diff routines. # Check ABA derivatives (without forces)
def compute_force(self, project_in_friction_cone): M = self.data.oMf[self.frame_id] self.p = M.translation delta_p = self.p0 - self.p R = se3.SE3(M.rotation, 0 * M.translation) v_local = se3.getFrameVelocity(self.model, self.data, self.frame_id) v_world = (R.act(v_local)).linear dJv_local = se3.getFrameAcceleration(self.model, self.data, self.frame_id) dJv_local.linear += np.cross(v_local.angular, v_local.linear, axis=0) dJv_world = (R.act(dJv_local)).linear self.v = v_world self.dJv = dJv_world if (self.slipping): self.dp0 = self.v - self.v.dot(self.normal) * self.normal self.f = self.K @ delta_p + self.B @ (self.dp0 - v_world) if (project_in_friction_cone): self.dp0 = zero(3) # check whether point is in contact if (self.f.dot(self.normal) <= 0.0): # print("\nINFO: Negative normal force %s!"%(self.frame_name), delta_p.T, self.f.T) self.f = zero(3) # do not reset v and dJv because they could be used by ExponentialSimulator # self.v = zero(3) # self.dJv = zero(3) else: # if(not self.active): # self.active = True # print("\nINFO: contact %s made!"%(self.frame_name)) # check whether contact force is outside friction cone f_N = self.f.dot(self.normal) # norm of normal force f_T = self.f - f_N * self.normal # tangential force (3d) f_T_norm = norm(f_T) # norm of tangential force if (f_T_norm > self.mu * f_N): # contact is slipping t_dir = f_T / f_T_norm # direction of tangential force # saturate force at the friction cone boundary f_T = self.mu * f_N * t_dir self.f = f_N * self.normal + f_T # update anchor point assuming anchor point vel is equal to contact point vel self.dp0 = self.v - self.v.dot(self.normal) * self.normal # f = K@(p0-p) + B@(v0-v) => p0 = p + f/K - B@(v0-v)/K self.p0 = self.p + self.Kinv @ ( self.f - self.B @ (self.dp0 - self.v)) if (self.slipping == False): self.slipping = True print( 'INFO: contact %s started slipping' % (self.frame_name), f_T_norm - self.mu * f_N) elif (self.slipping == True): self.slipping = False print( 'INFO: contact %s stopped slipping' % (self.frame_name), f_T_norm - self.mu * f_N) return self.f
def update(self, solo, qmes12, vmes12): """Update the quantities of the Interface based on the last measurements from the simulation Args: solo (object): Pinocchio wrapper for the quadruped qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators vmes12 (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators """ # Rotation matrix from the world frame to the base frame self.oRb = pin.Quaternion(qmes12[3:7]).matrix() # Linear and angular velocity in base frame self.vmes12_base = vmes12.copy() self.vmes12_base[0:3, 0:1] = self.oRb.transpose() @ self.vmes12_base[0:3, 0:1] self.vmes12_base[3:6, 0:1] = self.oRb.transpose() @ self.vmes12_base[3:6, 0:1] """# Update Kinematics (required or automatically done by other functions?) pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base) self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0] for i in self.indexes[1:]: self.mean_feet_z = np.min((self.mean_feet_z, solo.data.oMf[i].translation[2, 0])) qmes12[2, 0] -= self.mean_feet_z""" # Update Kinematics (required or automatically done by other functions?) pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base) pin.framesForwardKinematics(solo.model, solo.data, qmes12) # Get center of mass from Pinocchio pin.centerOfMass(solo.model, solo.data, qmes12, self.vmes12_base) # Update position/orientation of frames pin.updateFramePlacements(solo.model, solo.data) pin.ccrba(solo.model, solo.data, qmes12, self.vmes12_base) # Update minimum height of feet # TODO: Rename mean_feet_z into min_feet_z self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0] """for i in self.indexes: self.mean_feet_z += solo.data.oMf[i].translation[2, 0] self.mean_feet_z *= 0.25""" for i in self.indexes[1:]: self.mean_feet_z = np.min( (self.mean_feet_z, solo.data.oMf[i].translation[2, 0])) self.mean_feet_z = 0.0 # Store position, linear velocity and angular velocity in global frame self.oC = solo.data.com[0] self.oV = solo.data.vcom[0] self.oW = vmes12[3:6] pin.crba(solo.model, solo.data, qmes12) # Get SE3 object from world frame to base frame self.oMb = pin.SE3(pin.Quaternion(qmes12[3:7]), self.oC) self.RPY = pin.rpy.matrixToRpy(self.oMb.rotation) # Get SE3 object from world frame to local frame self.oMl = pin.SE3( pin.utils.rotate('z', self.RPY[2, 0]), np.array([qmes12[0, 0], qmes12[1, 0], self.mean_feet_z])) # Get position, linear velocity and angular velocity in local frame self.lC = self.oMl.inverse() * self.oC self.lV = self.oMl.rotation.transpose() @ self.oV self.lW = self.oMl.rotation.transpose() @ self.oW # Pos, vel and acc of feet for i, j in enumerate(self.indexes): # Position of feet in local frame self.o_feet[:, i:(i + 1)] = solo.data.oMf[j].translation self.l_feet[:, i:( i + 1)] = self.oMl.inverse() * solo.data.oMf[j].translation # getFrameVelocity output is in the frame of the foot so a transform is required self.ov_feet[:, i:( i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameVelocity( solo.model, solo.data, j).vector[0:3, 0:1] self.lv_feet[:, i:( i + 1 )] = self.oMl.rotation.transpose() @ self.ov_feet[:, i:(i + 1)] # getFrameAcceleration output is in the frame of the foot so a transform is required self.oa_feet[:, i:( i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameAcceleration( solo.model, solo.data, j).vector[0:3, 0:1] self.la_feet[:, i:( i + 1 )] = self.oMl.rotation.transpose() @ self.oa_feet[:, i:(i + 1)] # Orientation of the base in local frame # Base and local frames have the same yaw orientation in world frame self.abg[0:2] = self.RPY[0:2] # Position of shoulders in world frame for i in range(4): self.o_shoulders[:, i:(i + 1)] = self.oMl * self.l_shoulders[:, i] return 0
def compute(self, q, dq): ### FEET Jfeet = [] afeet = [] pfeet_err = [] vfeet_ref = [] pin.forwardKinematics(self.rmodel, self.rdata, q, dq, np.zeros(self.rmodel.nv)) pin.updateFramePlacements(self.rmodel, self.rdata) for i_ee in range(4): idx = int(self.foot_ids[i_ee]) pos = self.rdata.oMf[idx].translation nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED) ref = self.feet_position_ref[i_ee] vref = self.feet_velocity_ref[i_ee] aref = self.feet_acceleration_ref[i_ee] J1 = pin.computeFrameJacobian(self.robot.model, self.robot.data, q, idx, pin.LOCAL_WORLD_ALIGNED)[:3] e1 = ref - pos acc1 = -self.Kp_flyingfeet * (pos - ref) - self.Kd_flyingfeet * ( nu.linear - vref) + aref if self.flag_in_contact[i_ee]: acc1 *= 0 # In contact = no feedback drift1 = np.zeros(3) drift1 += pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).linear drift1 += self.cross3(nu.angular, nu.linear) acc1 -= drift1 Jfeet.append(J1) afeet.append(acc1) pfeet_err.append(e1) vfeet_ref.append(vref) ### BASE POSITION idx = self.BASE_ID pos = self.rdata.oMf[idx].translation nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED) ref = self.base_position_ref Jbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q, idx, pin.LOCAL_WORLD_ALIGNED)[:3] e_basispos = ref - pos accbasis = -self.Kp_base_position * ( pos - ref) - self.Kd_base_position * nu.linear drift = np.zeros(3) drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).linear drift += self.cross3(nu.angular, nu.linear) accbasis -= drift ### BASE ROTATION idx = self.BASE_ID rot = self.rdata.oMf[idx].rotation nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED) rotref = self.base_orientation_ref Jwbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q, idx, pin.LOCAL_WORLD_ALIGNED)[3:] e_basisrot = -rotref @ pin.log3(rotref.T @ rot) accwbasis = -self.Kp_base_orientation * rotref @ pin.log3( rotref.T @ rot) - self.Kd_base_orientation * nu.angular drift = np.zeros(3) drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).angular accwbasis -= drift J = np.vstack(Jfeet + [Jbasis, Jwbasis]) acc = np.concatenate(afeet + [accbasis, accwbasis]) x_err = np.concatenate(pfeet_err + [e_basispos, e_basisrot]) dx_ref = np.concatenate( vfeet_ref + [self.base_linearvelocity_ref, self.base_angularvelocity_ref]) invJ = self.dinv(J) #or np.linalg.inv(J) since full rank ddq = invJ @ acc self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err) self.dq_cmd = invJ @ dx_ref return ddq