| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2023 INRIA |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
#include "pinocchio/math/fwd.hpp" |
| 6 |
|
|
#include "pinocchio/multibody/joint/joints.hpp" |
| 7 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
| 8 |
|
|
#include "pinocchio/algorithm/aba.hpp" |
| 9 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
| 10 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
| 11 |
|
|
#include "pinocchio/algorithm/compute-all-terms.hpp" |
| 12 |
|
|
|
| 13 |
|
|
#include <boost/test/unit_test.hpp> |
| 14 |
|
|
#include <iostream> |
| 15 |
|
|
|
| 16 |
|
|
using namespace pinocchio; |
| 17 |
|
|
using namespace Eigen; |
| 18 |
|
|
|
| 19 |
|
|
template<typename D> |
| 20 |
|
✗ |
void addJointAndBody( |
| 21 |
|
|
Model & model, |
| 22 |
|
|
const JointModelBase<D> & jmodel, |
| 23 |
|
|
const Model::JointIndex parent_id, |
| 24 |
|
|
const SE3 & joint_placement, |
| 25 |
|
|
const std::string & joint_name, |
| 26 |
|
|
const Inertia & Y) |
| 27 |
|
|
{ |
| 28 |
|
|
Model::JointIndex idx; |
| 29 |
|
|
|
| 30 |
|
✗ |
idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name); |
| 31 |
|
✗ |
model.appendBodyToJoint(idx, Y); |
| 32 |
|
|
} |
| 33 |
|
|
|
| 34 |
|
|
BOOST_AUTO_TEST_SUITE(JointUniversal) |
| 35 |
|
|
|
| 36 |
|
✗ |
BOOST_AUTO_TEST_CASE(vsRXRY) |
| 37 |
|
|
{ |
| 38 |
|
|
typedef SE3::Vector3 Vector3; |
| 39 |
|
|
typedef SE3::Matrix3 Matrix3; |
| 40 |
|
|
|
| 41 |
|
✗ |
Vector3 axis1; |
| 42 |
|
✗ |
axis1 << 1.0, 0.0, 0.0; |
| 43 |
|
✗ |
Vector3 axis2; |
| 44 |
|
✗ |
axis2 << 0.0, 1.0, 0.0; |
| 45 |
|
|
|
| 46 |
|
✗ |
Model modelUniversal, modelRXRY; |
| 47 |
|
✗ |
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); |
| 48 |
|
|
|
| 49 |
|
✗ |
JointModelUniversal joint_model_U(axis1, axis2); |
| 50 |
|
✗ |
addJointAndBody(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia); |
| 51 |
|
|
|
| 52 |
|
✗ |
JointModelComposite joint_model_RXRY; |
| 53 |
|
✗ |
joint_model_RXRY.addJoint(JointModelRX()); |
| 54 |
|
✗ |
joint_model_RXRY.addJoint(JointModelRY()); |
| 55 |
|
✗ |
addJointAndBody(modelRXRY, joint_model_RXRY, 0, SE3::Identity(), "rxry", inertia); |
| 56 |
|
|
|
| 57 |
|
✗ |
Data dataUniversal(modelUniversal); |
| 58 |
|
✗ |
Data dataRXRY(modelRXRY); |
| 59 |
|
|
|
| 60 |
|
✗ |
BOOST_CHECK(modelUniversal.nv == modelRXRY.nv); |
| 61 |
|
✗ |
BOOST_CHECK(modelUniversal.nq == modelRXRY.nq); |
| 62 |
|
|
|
| 63 |
|
✗ |
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRXRY.nq); |
| 64 |
|
|
|
| 65 |
|
✗ |
forwardKinematics(modelRXRY, dataRXRY, q); |
| 66 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q); |
| 67 |
|
|
|
| 68 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back())); |
| 69 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back())); |
| 70 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix())); |
| 71 |
|
|
|
| 72 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRXRY.nv); |
| 73 |
|
✗ |
forwardKinematics(modelRXRY, dataRXRY, q, v); |
| 74 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q, v); |
| 75 |
|
|
|
| 76 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back())); |
| 77 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back())); |
| 78 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix())); |
| 79 |
|
|
|
| 80 |
|
✗ |
computeAllTerms(modelRXRY, dataRXRY, q, v); |
| 81 |
|
✗ |
computeAllTerms(modelUniversal, dataUniversal, q, v); |
| 82 |
|
|
|
| 83 |
|
✗ |
BOOST_CHECK(dataUniversal.com.back().isApprox(dataRXRY.com.back())); |
| 84 |
|
✗ |
BOOST_CHECK(dataUniversal.nle.isApprox(dataRXRY.nle)); |
| 85 |
|
✗ |
BOOST_CHECK(dataUniversal.f.back().toVector().isApprox(dataRXRY.f.back().toVector())); |
| 86 |
|
|
|
| 87 |
|
|
// InverseDynamics == rnea |
| 88 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRXRY.nv); |
| 89 |
|
|
|
| 90 |
|
✗ |
Eigen::VectorXd tauRXRY = rnea(modelRXRY, dataRXRY, q, v, a); |
| 91 |
|
✗ |
Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a); |
| 92 |
|
|
|
| 93 |
|
✗ |
BOOST_CHECK(tauUniversal.isApprox(tauRXRY)); |
| 94 |
|
|
|
| 95 |
|
|
// ForwardDynamics == aba |
| 96 |
|
✗ |
Eigen::VectorXd aAbaRXRY = aba(modelRXRY, dataRXRY, q, v, tauRXRY, Convention::WORLD); |
| 97 |
|
|
Eigen::VectorXd aAbaUniversal = |
| 98 |
|
✗ |
aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD); |
| 99 |
|
|
|
| 100 |
|
✗ |
BOOST_CHECK(aAbaUniversal.isApprox(aAbaRXRY)); |
| 101 |
|
|
|
| 102 |
|
|
// CRBA |
| 103 |
|
✗ |
crba(modelRXRY, dataRXRY, q, Convention::WORLD); |
| 104 |
|
✗ |
crba(modelUniversal, dataUniversal, q, Convention::WORLD); |
| 105 |
|
|
|
| 106 |
|
✗ |
BOOST_CHECK(dataUniversal.M.isApprox(dataRXRY.M)); |
| 107 |
|
|
|
| 108 |
|
|
// Jacobian |
| 109 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRXRY; |
| 110 |
|
✗ |
jacobianRXRY.resize(6, 2); |
| 111 |
|
✗ |
jacobianRXRY.setZero(); |
| 112 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal; |
| 113 |
|
✗ |
jacobianUniversal.resize(6, 2); |
| 114 |
|
✗ |
jacobianUniversal.setZero(); |
| 115 |
|
|
|
| 116 |
|
✗ |
computeJointJacobians(modelRXRY, dataRXRY, q); |
| 117 |
|
✗ |
computeJointJacobians(modelUniversal, dataUniversal, q); |
| 118 |
|
✗ |
getJointJacobian(modelRXRY, dataRXRY, 1, LOCAL, jacobianRXRY); |
| 119 |
|
✗ |
getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal); |
| 120 |
|
|
|
| 121 |
|
✗ |
BOOST_CHECK(jacobianUniversal.isApprox(jacobianRXRY)); |
| 122 |
|
|
} |
| 123 |
|
|
|
| 124 |
|
✗ |
BOOST_AUTO_TEST_CASE(vsRandomAxis) |
| 125 |
|
|
{ |
| 126 |
|
|
typedef SE3::Vector3 Vector3; |
| 127 |
|
|
typedef SE3::Matrix3 Matrix3; |
| 128 |
|
|
|
| 129 |
|
✗ |
Vector3 axis1; |
| 130 |
|
✗ |
axis1 << 0., 0., 1.; |
| 131 |
|
✗ |
Vector3 axis2; |
| 132 |
|
✗ |
axis2 << -1., 0., 0.; |
| 133 |
|
|
|
| 134 |
|
✗ |
Model modelUniversal, modelRandomAxis; |
| 135 |
|
✗ |
Inertia inertia = Inertia::Random(); |
| 136 |
|
|
|
| 137 |
|
✗ |
JointModelUniversal joint_model_U(axis1, axis2); |
| 138 |
|
✗ |
addJointAndBody(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia); |
| 139 |
|
|
|
| 140 |
|
✗ |
JointModelComposite joint_model_RandomAxis; |
| 141 |
|
✗ |
joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis1)); |
| 142 |
|
✗ |
joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis2)); |
| 143 |
|
✗ |
addJointAndBody( |
| 144 |
|
✗ |
modelRandomAxis, joint_model_RandomAxis, 0, SE3::Identity(), "random_axis", inertia); |
| 145 |
|
|
|
| 146 |
|
✗ |
Data dataUniversal(modelUniversal); |
| 147 |
|
✗ |
Data dataRandomAxis(modelRandomAxis); |
| 148 |
|
|
|
| 149 |
|
✗ |
BOOST_CHECK(modelUniversal.nv == modelRandomAxis.nv); |
| 150 |
|
✗ |
BOOST_CHECK(modelUniversal.nq == modelRandomAxis.nq); |
| 151 |
|
|
|
| 152 |
|
✗ |
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRandomAxis.nq); |
| 153 |
|
|
|
| 154 |
|
✗ |
forwardKinematics(modelRandomAxis, dataRandomAxis, q); |
| 155 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q); |
| 156 |
|
|
|
| 157 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back())); |
| 158 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back())); |
| 159 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix())); |
| 160 |
|
|
|
| 161 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRandomAxis.nv); |
| 162 |
|
✗ |
forwardKinematics(modelRandomAxis, dataRandomAxis, q, v); |
| 163 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q, v); |
| 164 |
|
|
|
| 165 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back())); |
| 166 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back())); |
| 167 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix())); |
| 168 |
|
|
|
| 169 |
|
✗ |
computeAllTerms(modelRandomAxis, dataRandomAxis, q, v); |
| 170 |
|
✗ |
computeAllTerms(modelUniversal, dataUniversal, q, v); |
| 171 |
|
|
|
| 172 |
|
✗ |
BOOST_CHECK(dataUniversal.com.back().isApprox(dataRandomAxis.com.back())); |
| 173 |
|
✗ |
BOOST_CHECK(dataUniversal.nle.isApprox(dataRandomAxis.nle)); |
| 174 |
|
✗ |
BOOST_CHECK(dataUniversal.f.back().toVector().isApprox(dataRandomAxis.f.back().toVector())); |
| 175 |
|
|
|
| 176 |
|
|
// InverseDynamics == rnea |
| 177 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRandomAxis.nv); |
| 178 |
|
|
|
| 179 |
|
✗ |
Eigen::VectorXd tauRandomAxis = rnea(modelRandomAxis, dataRandomAxis, q, v, a); |
| 180 |
|
✗ |
Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a); |
| 181 |
|
|
|
| 182 |
|
✗ |
BOOST_CHECK(tauUniversal.isApprox(tauRandomAxis)); |
| 183 |
|
|
|
| 184 |
|
|
// ForwardDynamics == aba |
| 185 |
|
|
Eigen::VectorXd aAbaRandomAxis = |
| 186 |
|
✗ |
aba(modelRandomAxis, dataRandomAxis, q, v, tauRandomAxis, Convention::WORLD); |
| 187 |
|
|
Eigen::VectorXd aAbaUniversal = |
| 188 |
|
✗ |
aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD); |
| 189 |
|
|
|
| 190 |
|
✗ |
BOOST_CHECK(aAbaUniversal.isApprox(aAbaRandomAxis)); |
| 191 |
|
|
|
| 192 |
|
|
// CRBA |
| 193 |
|
✗ |
crba(modelRandomAxis, dataRandomAxis, q, Convention::WORLD); |
| 194 |
|
✗ |
crba(modelUniversal, dataUniversal, q, Convention::WORLD); |
| 195 |
|
|
|
| 196 |
|
✗ |
BOOST_CHECK(dataUniversal.M.isApprox(dataRandomAxis.M)); |
| 197 |
|
|
|
| 198 |
|
|
// Jacobian |
| 199 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRandomAxis; |
| 200 |
|
✗ |
jacobianRandomAxis.resize(6, 2); |
| 201 |
|
✗ |
jacobianRandomAxis.setZero(); |
| 202 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal; |
| 203 |
|
✗ |
jacobianUniversal.resize(6, 2); |
| 204 |
|
✗ |
jacobianUniversal.setZero(); |
| 205 |
|
|
|
| 206 |
|
✗ |
computeJointJacobians(modelRandomAxis, dataRandomAxis, q); |
| 207 |
|
✗ |
computeJointJacobians(modelUniversal, dataUniversal, q); |
| 208 |
|
✗ |
getJointJacobian(modelRandomAxis, dataRandomAxis, 1, LOCAL, jacobianRandomAxis); |
| 209 |
|
✗ |
getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal); |
| 210 |
|
|
|
| 211 |
|
✗ |
BOOST_CHECK(jacobianUniversal.isApprox(jacobianRandomAxis)); |
| 212 |
|
|
} |
| 213 |
|
|
|
| 214 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
| 215 |
|
|
|