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_velocity(robot: jiminy.Model, body_name: str, use_theoretical_model: bool = True) -> pin.SE3: """Get the spatial velocity wrt world in body frame for a given body. .. warning:: It is assumed that `update_quantities` has been called beforehand. :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 velocity. """ # 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.getFrameVelocity( pnc_model, pnc_data, body_id, pin.LOCAL_WORLD_ALIGNED)
def BaseVelocityFromKinAndIMU(self, contactFrameId): """Estimate the velocity of the base with forward kinematics using a contact point that is supposed immobile in world frame Args: contactFrameId (int): ID of the contact point frame (foot frame) """ frameVelocity = pin.getFrameVelocity(self.model, self.data, contactFrameId, pin.ReferenceFrame.LOCAL) framePlacement = pin.updateFramePlacement(self.model, self.data, contactFrameId) # Angular velocity of the base wrt the world in the base frame (Gyroscope) _1w01 = self.IMU_ang_vel.reshape((3, 1)) # Linear velocity of the foot wrt the base in the foot frame _Fv1F = frameVelocity.linear # Level arm between the base and the foot _1F = np.array(framePlacement.translation) # Orientation of the foot wrt the base _1RF = framePlacement.rotation # Linear velocity of the base wrt world in the base frame _1v01 = self.cross3(_1F.ravel(), _1w01.ravel()) - \ (_1RF @ _Fv1F.reshape((3, 1))) # IMU and base frames have the same orientation # _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel()) return np.array(_1v01)
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) # Perform the forward kinematics over the kinematic tree forwardKinematics(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel) computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_linear_vel = joint_velocity.linear joint_angular_vel = joint_velocity.angular joint_6v_vel = joint_velocity.vector err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_linear_vel = frame_velocity.linear frame_angular_vel = frame_velocity.angular frame_6v_vel = frame_velocity.vector err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def get_velocity(self): M = self.data.oMf[self.frame_id] R = se3.SE3( M.rotation, 0 * M.translation) # same as M but with translation set to zero v_local = se3.getFrameVelocity(self.model, self.data, self.frame_id) v_world = (R.act(v_local) ).linear # convert velocity from local frame to world frame return v_world
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 get_link_vel(self, link_id): ret = np.zeros(6) frame_id = self._model.getFrameId(link_id) spatial_vel = pin.getFrameVelocity( self._model, self._data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) ret[0:3] = spatial_vel.angular ret[3:6] = spatial_vel.linear return np.copy(ret)
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) numpy_vector_joint_acc = np.random.rand(model.njoints - 1) # Calls Reverese Newton-Euler algorithm numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) # IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = joint_velocity.vector - \ joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = frame_velocity.vector - \ frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def createEESplines(self, rmodel, rdata, xs, t_sampling=0.005): N = len(xs) abscissa = a2m(np.linspace(0., t_sampling * (N - 1), N)) self.ee_splines = EESplines() for patch in self.ee_map.keys(): p = np.zeros((3, N)) m = np.zeros((3, N)) for i in range(N): q = a2m(xs[i][:rmodel.nq]) v = a2m(xs[i][-rmodel.nv:]) pinocchio.forwardKinematics(rmodel, rdata, q, v) p[:, i] = m2a( pinocchio.updateFramePlacement(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).translation) m[:, i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).linear) self.ee_splines.update([[patch, CubicHermiteSpline(abscissa, p, m)]]) return
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): 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 forward_kinematics( self, joint_positions: typing.List[np.ndarray], joint_velocities: typing.Optional[np.ndarray] = None, ) -> typing.Union[typing.List[np.ndarray], typing.Tuple[ typing.List[np.ndarray], typing.List[np.ndarray]], ]: """Compute end-effector positions (and velocities) for the given joint configuration. Args: joint_positions: Flat list of angular joint positions. joint_velocities: Optional. Flat list of angular joint velocities. Returns: If only joint positions are given: List of end-effector positions. Each position is given as an np.array with x,y,z positions. If joint positions and velocities are given: Tuple with (i) list of end-effector positions and (ii) list of end-effector velocities. Each position and velocity is given as an np.array with x,y,z components. """ pinocchio.framesForwardKinematics(self.robot_model, self.data, joint_positions) positions = [ np.asarray( self.data.oMf[link_id].translation).reshape(-1).tolist() for link_id in self.tip_link_ids ] if joint_velocities is None: return positions else: pinocchio.forwardKinematics(self.robot_model, self.data, joint_positions, joint_velocities) velocities = [] for link_id in self.tip_link_ids: local_to_world_transform = pinocchio.SE3.Identity() local_to_world_transform.rotation = self.data.oMf[ link_id].rotation v_local = pinocchio.getFrameVelocity(self.robot_model, self.data, link_id) v_world = local_to_world_transform.act(v_local) velocities.append(v_world.linear) return positions, velocities
def ForwardKinematics(self, q, v=np.zeros(3)): """ Description: 1. Computes the Cartesian Position & Velocity of the robot 2. The TCP position is w.r.t. the 'tip_link' of the robot. Args: q -> np.array (3,) : Joint Positions of the robot. v -> np.array (3,) : Joint Velocities of the robot. Returns: np.array (3,) : Cartesian Position of the robot. np.array (3,) : Cartesian Velocity of the robot. """ index = self.EOAT_ID frame = pin.ReferenceFrame.LOCAL pin.forwardKinematics(self.model, self.model_data, q, v) pin.updateFramePlacements(self.model, self.model_data) pos = pin.updateFramePlacement(self.model, self.model_data, index) vel = pin.getFrameVelocity(self.model, self.model_data, index, frame) return np.array(pos)[:3, -1], np.array(vel)[:3]
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)
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 calc(self, data, x, u): data.r = (pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.vref.frame) - self.vref.oMf).vector self.activation.calc(data.activation, data.r) data.cost = data.activation.a
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
def calc(self, data, x, u): data.residuals[:] = m2a( pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio, self.frame).linear) - self.ref data.cost = sum(self.activation.calc(data.activation, data.residuals)) return data.cost
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 get_frame_velocity(self, q, v, frame_id) -> pin.Motion: pin.forwardKinematics(self.model, self.data, q, v) return pin.getFrameVelocity(self.model, self.data, self.model.getFrameId(frame_id), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def writeToMessage(self, t, q, v=None, tau=None, p=dict(), pd=dict(), f=dict(), s=dict()): # Filling the time information self._msg.header.stamp = rospy.Time(t) self._msg.time = t if v is None: v = np.zeros(self._model.nv) if tau is None: tau = np.zeros(self._model.njoints - 2) # Filling the centroidal state pinocchio.centerOfMass(self._model, self._data, q, v) c = self._data.com[0] cd = self._data.vcom[0] # Center of mass self._msg.centroidal.com_position.x = c[0] self._msg.centroidal.com_position.y = c[1] self._msg.centroidal.com_position.z = c[2] self._msg.centroidal.com_velocity.x = cd[0] self._msg.centroidal.com_velocity.y = cd[1] self._msg.centroidal.com_velocity.z = cd[2] # Base self._msg.centroidal.base_orientation.x = q[3] self._msg.centroidal.base_orientation.y = q[4] self._msg.centroidal.base_orientation.z = q[5] self._msg.centroidal.base_orientation.w = q[6] self._msg.centroidal.base_angular_velocity.x = v[3] self._msg.centroidal.base_angular_velocity.y = v[4] self._msg.centroidal.base_angular_velocity.z = v[5] # Momenta momenta = pinocchio.computeCentroidalMomentum(self._model, self._data) momenta_rate = pinocchio.computeCentroidalMomentumTimeVariation( self._model, self._data) self._msg.centroidal.momenta.linear.x = momenta.linear[0] self._msg.centroidal.momenta.linear.y = momenta.linear[1] self._msg.centroidal.momenta.linear.z = momenta.linear[2] self._msg.centroidal.momenta.angular.x = momenta.angular[0] self._msg.centroidal.momenta.angular.y = momenta.angular[1] self._msg.centroidal.momenta.angular.z = momenta.angular[2] self._msg.centroidal.momenta_rate.linear.x = momenta_rate.linear[0] self._msg.centroidal.momenta_rate.linear.y = momenta_rate.linear[1] self._msg.centroidal.momenta_rate.linear.z = momenta_rate.linear[2] self._msg.centroidal.momenta_rate.angular.x = momenta_rate.angular[0] self._msg.centroidal.momenta_rate.angular.y = momenta_rate.angular[1] self._msg.centroidal.momenta_rate.angular.z = momenta_rate.angular[2] # Filling the joint state njoints = self._model.njoints - 2 for j in range(njoints): self._msg.joints[j].position = q[7 + j] self._msg.joints[j].velocity = v[6 + j] self._msg.joints[j].effort = tau[j] # Filling the contact state names = list(p.keys()) + list(pd.keys()) + list(f.keys()) + list( s.keys()) names = list(dict.fromkeys(names)) self._msg.contacts = [None] * len(names) if len(names) != 0 and (len(p.keys()) == 0 or len(pd.keys()) == 0): pinocchio.forwardKinematics(self._model, self._data, q, v) for i, name in enumerate(names): contact_msg = ContactState() contact_msg.name = name frame_id = self._model.getFrameId(name) # Retrive the contact position if name in p: pose = pinocchio.SE3ToXYZQUAT(p[name]) else: oMf = pinocchio.updateFramePlacement(self._model, self._data, frame_id) pose = pinocchio.SE3ToXYZQUAT(oMf) # Retrieve the contact velocity if name in pd: ovf = pd[name] else: ovf = pinocchio.getFrameVelocity( self._model, self._data, frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED) # Storing the contact position and velocity inside the message contact_msg.pose.position.x = pose[0] contact_msg.pose.position.y = pose[1] contact_msg.pose.position.z = pose[2] contact_msg.pose.orientation.x = pose[3] contact_msg.pose.orientation.y = pose[4] contact_msg.pose.orientation.z = pose[5] contact_msg.pose.orientation.w = pose[6] contact_msg.velocity.linear.x = ovf.linear[0] contact_msg.velocity.linear.y = ovf.linear[1] contact_msg.velocity.linear.z = ovf.linear[2] contact_msg.velocity.angular.x = ovf.angular[0] contact_msg.velocity.angular.y = ovf.angular[1] contact_msg.velocity.angular.z = ovf.angular[2] # Retrieving and storing force data if name in f: contact_info = f[name] ctype = contact_info[0] force = contact_info[1] contact_msg.type = ctype contact_msg.wrench.force.x = force.linear[0] contact_msg.wrench.force.y = force.linear[1] contact_msg.wrench.force.z = force.linear[2] contact_msg.wrench.torque.x = force.angular[0] contact_msg.wrench.torque.y = force.angular[1] contact_msg.wrench.torque.z = force.angular[2] if name in s: terrain_info = s[name] norm = terrain_info[0] friction = terrain_info[1] contact_msg.surface_normal.x = norm[0] contact_msg.surface_normal.y = norm[1] contact_msg.surface_normal.z = norm[2] contact_msg.friction_coefficient = friction self._msg.contacts[i] = contact_msg return copy.deepcopy(self._msg)
## Calculate l2 placement l2_frame = model.getFrameId('l2') l2_translation = pin.updateFramePlacement(model, data, l2_frame) print("l2 translation") print(l2_translation) ## Calculate j2 jacobian pin.computeJointJacobians(model, data, q) j2_jacobian = pin.getFrameJacobian(model, data, j2_frame, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) print("j2 jacobian") print(j2_jacobian) ## Calculate l2 jacobian l2_jacobian = pin.getFrameJacobian(model, data, l2_frame, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) print("l2 jacobian") print(l2_jacobian) ## Calculate j2 spatial velocity j2_vel = pin.getFrameVelocity(model, data, j2_frame) print("j2 vel") print(j2_vel) ## Calculate l2 spatial velocity l2_vel = pin.getFrameVelocity(model, data, l2_frame, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) print("l2 vel") print(l2_vel) print(np.dot(l2_jacobian, qdot))