learnpino: A C++ repository from xiaosuyang

建立模型

  namespace pino=pinocchio;
  const std::string urdf_filename=PINOCCHIO_MODEL_DIR;
  pino::Model model;
  pino::urdf::buildModel(urdf_filename,model);

  // Create data required by the algorithms
  pino::Data data(model);

Jointvarient

 typedef boost::variant<
//    JointModelVoid,
      JointModelRX, JointModelRY, JointModelRZ
    , JointModelMimicRX, JointModelMimicRY, JointModelMimicRZ
    , JointModelFreeFlyer, JointModelPlanar
    , JointModelRevoluteUnaligned
    , JointModelSpherical, JointModelSphericalZYX
    , JointModelPX, JointModelPY, JointModelPZ
    , JointModelPrismaticUnaligned
    , JointModelTranslation
    , JointModelRUBX, JointModelRUBY, JointModelRUBZ
    , JointModelRevoluteUnboundedUnaligned
    , boost::recursive_wrapper<JointModelComposite>
    > JointModelVariant;

Boost.Variant 提供了一个类似于 union 的名为 boost::variant 的类。您可以将不同类型的值存储在 boost::variant 变量中。在任何时候只能存储一个值。分配新值时，旧值将被覆盖。但是，新值的类型可能与旧值不同。唯一的要求是这些类型必须作为模板参数传递给 boost::variant，这样它们才能为 boost::variant 变量所知。

#include <boost/variant.hpp>
#include <string>
 
int main()
{
  boost::variant<double, char, std::string> v;
  v = 3.14;
  v = 'A';
  v = "Boost";
}

浮动基

q = [global_base_position, global_base_quaternion, joint_positions]
v = [local_base_velocity_linear, local_base_velocity_angular, joint_velocities]

The base translation part is expressed in the parent frame (here the world coordinate system) while its velocity is expressed in the body coordinate system.

rpy转四元数方法

Eigen::Matrix3d rotationMatrix = pinocchio::rpyToMatrix(roll, pitch, yaw);

    Eigen::Quaterniond quaternion(rotationMatrix);

SE（3）

$$ \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}\in R^{4\times4} $$

表示方向,两个frame的相对orientation,B在A中表示: $$ ^AR_B=\begin{bmatrix}^A\hat{x}_B &^A\hat{y}_B&^A\hat{z}_B\end{bmatrix} $$

 data.oMi[joint_id];
 data.oMi[joint_id].translation()
 data.oMi[joint_id].rotation()

SO(3)

$$ so(3)={R\in R^{3\times3}:R^TR=I,det(R)=1} $$

表示变换: $$ ^AC_B=^AR_B $$

Frame坐标转换

$$ m\in M^6,f\in F^6 \newline m_B=^BX_Am_A,f_B=^BX_A^Ff_A $$ Frame：universe,world,root_joint,以及urdf中所有joint和link的坐标

ComputeAllterms

包含如下计算：

pinocchio::forwardKinematics

Update the joint placements，spatial velocities ,spatial accelerations according to the current joint configuration,velocity,acceleration。

pinocchio::crba

joint-space formulation: $$ H(q)\ddot{q}+C(q,\dot{q})\dot{q}+\tau_g(q)=\tau $$

$$ \begin{bmatrix} \tau_f \ \tau_j \ \end{bmatrix}=H(q)\ddot{q}+C(q,\dot{q})+g-J_c^Tf_r $$

计算JSIM上三角部分,下三角部分:
```
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
```

pinocchio::nonLinearEffects

Calculate Corriolis, centrifual and gravitationnal effects
pinocchio::rnea(model,data,q,v,0)

pinocchio::computeJointJacobians

pinocchio::centerOfMass()

return The center of mass position of the full rigid body system expressed in the world frame.

Parameters

	model	The model structure of the rigid body system.
	data	The data structure of the rigid body system.
	q	The joint configuration vector (dim model.nq).
[	computeSubtreeComs	If true, the algorithm computes also the center of mass of the subtrees.

pinocchio::ccrba

centroidal composit-rigid-body algorithm
返回质心动量矩阵$A_G$ $$ h_G=A_G(q)\dot{q} $$

pinocchio::UpdataFramePlacement

更新各个Frame的位置(pino::forwardkinematics更新各个joint的位置)

Pinocchio::Datatpl

data.M. The joint space inertia matrix (a square matrix of dim model.nv).
data.nle.In the multibody dynamics equation $M\ddot{q}+b(q,\dot{q})=\tau$, term $b$

Pinocchio::Frametpl

placement(SE3):Placement of the frame wrt the parent joint.

xiaosuyang/learnpino