Skip to content

Commit ff736aa

Browse files
rubengrandiarubengrandia
committed
add CoM computation
1 parent c178a11 commit ff736aa

File tree

3 files changed

+130
-16
lines changed

3 files changed

+130
-16
lines changed

‎ocs2_anymal_models/include/ocs2_anymal_models/QuadrupedCom.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,11 @@ class QuadrupedCom : public switched_model::ComModelBase<SCALAR_T> {
2121

2222
SCALAR_T totalMass() const override { return totalMass_; }
2323

24+
switched_model::vector3_s_t<SCALAR_T> centerOfMassInBaseFrame(const switched_model::joint_coordinate_s_t<SCALAR_T>& jointPositions) const;
25+
26+
switched_model::vector3_s_t<SCALAR_T> centerOfMassInWorldFrame(const switched_model::base_coordinate_s_t<SCALAR_T>& basePose,
27+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointPositions) const;
28+
2429
switched_model::base_coordinate_s_t<SCALAR_T> calculateBaseLocalAccelerations(
2530
const switched_model::base_coordinate_s_t<SCALAR_T>& basePose,
2631
const switched_model::base_coordinate_s_t<SCALAR_T>& baseLocalVelocities,
@@ -32,6 +37,14 @@ class QuadrupedCom : public switched_model::ComModelBase<SCALAR_T> {
3237
private:
3338
QuadrupedCom(const QuadrupedCom& rhs);
3439

40+
Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1> getPinnochioConfiguration(
41+
const switched_model::base_coordinate_s_t<SCALAR_T>& basePose,
42+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointPositions) const;
43+
44+
Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1> getPinnochioVelocity(
45+
const switched_model::base_coordinate_s_t<SCALAR_T>& baseLocalVelocities,
46+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointVelocities) const;
47+
3548
using PinocchioInterface_s_t = ocs2::PinocchioInterfaceTpl<SCALAR_T>;
3649

3750
template <typename T = SCALAR_T, typename std::enable_if<std::is_same<T, ocs2::ad_scalar_t>::value, bool>::type = true>

‎ocs2_anymal_models/src/QuadrupedCom.cpp‎

Lines changed: 56 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,26 @@ QuadrupedCom<SCALAR_T>* QuadrupedCom<SCALAR_T>::clone() const {
4040
return new QuadrupedCom<SCALAR_T>(*this);
4141
}
4242

43+
template <typename SCALAR_T>
44+
switched_model::vector3_s_t<SCALAR_T> QuadrupedCom<SCALAR_T>::centerOfMassInBaseFrame(
45+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointPositions) const {
46+
auto& data = pinocchioInterfacePtr_->getData();
47+
const auto& model = pinocchioInterfacePtr_->getModel();
48+
const auto configuration = getPinnochioConfiguration(switched_model::base_coordinate_s_t<SCALAR_T>::Zero(), jointPositions);
49+
pinocchio::centerOfMass(model, data, configuration);
50+
return data.com[1]; // CoM of the full robot in the free-flyer frame, i.e. base frame.
51+
}
52+
53+
template <typename SCALAR_T>
54+
switched_model::vector3_s_t<SCALAR_T> QuadrupedCom<SCALAR_T>::centerOfMassInWorldFrame(
55+
const switched_model::base_coordinate_s_t<SCALAR_T>& basePose,
56+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointPositions) const {
57+
auto& data = pinocchioInterfacePtr_->getData();
58+
const auto& model = pinocchioInterfacePtr_->getModel();
59+
const auto configuration = getPinnochioConfiguration(basePose, jointPositions);
60+
return pinocchio::centerOfMass(model, data, configuration);
61+
}
62+
4363
template <typename SCALAR_T>
4464
switched_model::base_coordinate_s_t<SCALAR_T> QuadrupedCom<SCALAR_T>::calculateBaseLocalAccelerations(
4565
const switched_model::base_coordinate_s_t<SCALAR_T>& basePose, const switched_model::base_coordinate_s_t<SCALAR_T>& baseLocalVelocities,
@@ -64,25 +84,12 @@ switched_model::base_coordinate_s_t<SCALAR_T> QuadrupedCom<SCALAR_T>::calculateB
6484
* w: Base angular velocity in Base Frame (3x1)
6585
* qj: Joint velocities per leg [HAA, HFE, KFE] (3x1)
6686
*/
67-
vector_t configuration = vector_t::Zero(model.nq);
68-
// Leave basePos empty here - Not necessary to fill
69-
// baseQuad
70-
const Eigen::Quaternion<SCALAR_T> baseQuat = switched_model::quaternionBaseToOrigin<SCALAR_T>(switched_model::getOrientation(basePose));
71-
configuration.template segment<4>(3) = baseQuat.coeffs();
72-
// JointsPos
73-
configuration.template segment<switched_model::JOINT_COORDINATE_SIZE>(7) = pinocchioMapping_.getPinocchioJointVector(jointPositions);
87+
const auto configuration = getPinnochioConfiguration(basePose, jointPositions);
88+
const auto velocity = getPinnochioVelocity(baseLocalVelocities, jointVelocities);
7489

7590
// Calculate joint space inertial matrix
7691
pinocchio::crba(model, data, configuration);
7792

78-
vector_t velocity = vector_t::Zero(model.nv);
79-
// Base linear velocity in Base frame
80-
velocity.template head<3>() = switched_model::getLinearVelocity(baseLocalVelocities);
81-
// Base angular velocity in Base frame
82-
velocity.template segment<3>(3) = switched_model::getAngularVelocity(baseLocalVelocities);
83-
// Joint velocity
84-
velocity.template segment<switched_model::JOINT_COORDINATE_SIZE>(6) = pinocchioMapping_.getPinocchioJointVector(jointVelocities);
85-
8693
const vector_t dynamicBias = pinocchio::nonLinearEffects(model, data, configuration, velocity);
8794

8895
switched_model::base_coordinate_s_t<SCALAR_T> pinocchioBaseForces;
@@ -107,6 +114,40 @@ switched_model::base_coordinate_s_t<SCALAR_T> QuadrupedCom<SCALAR_T>::calculateB
107114
return ocs2baseAcceleration;
108115
}
109116

117+
template <typename SCALAR_T>
118+
Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1> QuadrupedCom<SCALAR_T>::getPinnochioConfiguration(
119+
const switched_model::base_coordinate_s_t<SCALAR_T>& basePose,
120+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointPositions) const {
121+
const auto& model = pinocchioInterfacePtr_->getModel();
122+
123+
Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1> configuration(model.nq);
124+
// basePost
125+
configuration.template head<3>() = switched_model::getPositionInOrigin(basePose);
126+
// baseQuad
127+
const Eigen::Quaternion<SCALAR_T> baseQuat = switched_model::quaternionBaseToOrigin<SCALAR_T>(switched_model::getOrientation(basePose));
128+
configuration.template segment<4>(3) = baseQuat.coeffs();
129+
// JointsPos
130+
configuration.template segment<switched_model::JOINT_COORDINATE_SIZE>(7) = pinocchioMapping_.getPinocchioJointVector(jointPositions);
131+
return configuration;
132+
}
133+
134+
template <typename SCALAR_T>
135+
Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1> QuadrupedCom<SCALAR_T>::getPinnochioVelocity(
136+
const switched_model::base_coordinate_s_t<SCALAR_T>& baseLocalVelocities,
137+
const switched_model::joint_coordinate_s_t<SCALAR_T>& jointVelocities) const {
138+
const auto& model = pinocchioInterfacePtr_->getModel();
139+
140+
Eigen::Matrix<SCALAR_T, Eigen::Dynamic, 1> velocity(model.nv);
141+
// Base linear velocity in Base frame
142+
velocity.template head<3>() = switched_model::getLinearVelocity(baseLocalVelocities);
143+
// Base angular velocity in Base frame
144+
velocity.template segment<3>(3) = switched_model::getAngularVelocity(baseLocalVelocities);
145+
// Joint velocity
146+
velocity.template segment<switched_model::JOINT_COORDINATE_SIZE>(6) = pinocchioMapping_.getPinocchioJointVector(jointVelocities);
147+
148+
return velocity;
149+
}
150+
110151
} // namespace tpl
111152
} // namespace anymal
112153

‎ocs2_anymal_models/test/TestQuadrupedPinocchioCom.cpp‎

Lines changed: 61 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,13 @@
11
#include <gtest/gtest.h>
22

3+
#include <ocs2_pinocchio_interface/urdf.h>
4+
5+
#include <ocs2_switched_model_interface/core/Rotations.h>
6+
37
#include <ocs2_anymal_models/AnymalModels.h>
48
#include <ocs2_anymal_models/QuadrupedCom.h>
9+
#include <ocs2_anymal_models/QuadrupedInverseKinematics.h>
10+
#include <ocs2_anymal_models/QuadrupedKinematics.h>
511
#include <ocs2_anymal_models/package_path.h>
612

713
#include "camel/AnymalCamelCom.h"
@@ -13,14 +19,18 @@ class QuadrupedComTest : public ::testing::Test {
1319
QuadrupedComTest()
1420
: frameDeclaration(frameDeclarationFromFile(getPath() + "/urdf/frame_declaration_anymal_c.info")),
1521
pinocchioInterface(createQuadrupedPinocchioInterfaceFromUrdfString(getUrdfString(AnymalModel::Camel))),
16-
pinocchioCom_(frameDeclaration, pinocchioInterface) {
22+
pinocchioCom_(frameDeclaration, pinocchioInterface),
23+
kinematics_(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(getUrdfString(AnymalModel::Camel))),
24+
inverseKinematics_(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(getUrdfString(AnymalModel::Camel))) {
1725
srand(0);
1826
}
1927

2028
FrameDeclaration frameDeclaration;
2129
ocs2::PinocchioInterface pinocchioInterface;
2230
QuadrupedCom pinocchioCom_;
2331
AnymalCamelCom robcogenCom_;
32+
QuadrupedKinematics kinematics_;
33+
QuadrupedInverseKinematics inverseKinematics_;
2434
};
2535

2636
TEST_F(QuadrupedComTest, totalMass) {
@@ -43,3 +53,53 @@ TEST_F(QuadrupedComTest, calculateBaseLocalAccelerations) {
4353

4454
EXPECT_TRUE(pAcc.isApprox(rAcc, 1e-8)) << "Pinocchio: \n" << pAcc.transpose() << "\nRoboGen: \n" << rAcc.transpose();
4555
}
56+
57+
TEST_F(QuadrupedComTest, comWorldAndBase) {
58+
const switched_model::base_coordinate_t basePose = switched_model::base_coordinate_t::Random();
59+
const switched_model::joint_coordinate_t jointPositions = switched_model::joint_coordinate_t::Random();
60+
61+
const auto comInWorld = pinocchioCom_.centerOfMassInWorldFrame(basePose, jointPositions);
62+
const auto comInBase = pinocchioCom_.centerOfMassInBaseFrame(jointPositions);
63+
const switched_model::vector3_t comInWorldCheck =
64+
switched_model::rotateVectorBaseToOrigin(comInBase, switched_model::getOrientation(basePose)) +
65+
switched_model::getPositionInOrigin(basePose);
66+
67+
ASSERT_TRUE(comInWorld.isApprox(comInWorldCheck));
68+
}
69+
70+
TEST_F(QuadrupedComTest, comInBasePerConfiguration) {
71+
switched_model::base_coordinate_t basePose = switched_model::base_coordinate_t::Zero();
72+
switched_model::joint_coordinate_t jointPositionsDefault;
73+
jointPositionsDefault << -0.25, 0.60, -0.85, 0.25, 0.60, -0.85, -0.25, -0.60, 0.85, 0.25, -0.60, 0.85;
74+
75+
const auto baseToFeetDefault = kinematics_.positionBaseToFeetInBaseFrame(jointPositionsDefault);
76+
auto hipToFeetDefault = baseToFeetDefault;
77+
for (int leg = 0; leg < switched_model::NUM_CONTACT_POINTS; ++leg) {
78+
hipToFeetDefault[leg] -= kinematics_.baseToLegRootInBaseFrame(leg);
79+
}
80+
81+
for (ocs2::scalar_t pitchDeg = -50; pitchDeg < 51.0; pitchDeg += 5.0) {
82+
ocs2::scalar_t pitch = pitchDeg * (M_PI / 180.0);
83+
basePose[1] = pitch;
84+
85+
const auto comInWorldDefault = pinocchioCom_.centerOfMassInWorldFrame(basePose, jointPositionsDefault);
86+
const auto comInBaseDefault = pinocchioCom_.centerOfMassInBaseFrame(jointPositionsDefault);
87+
88+
// Adaptive legs
89+
switched_model::joint_coordinate_t jointPositionsAdaptive;
90+
for (int leg = 0; leg < switched_model::NUM_CONTACT_POINTS; ++leg) {
91+
switched_model::vector3_t baseToFootInBase =
92+
kinematics_.baseToLegRootInBaseFrame(leg) +
93+
switched_model::rotateVectorOriginToBase(hipToFeetDefault[leg], switched_model::getOrientation(basePose));
94+
jointPositionsAdaptive.segment<3>(3 * leg) =
95+
inverseKinematics_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(leg, baseToFootInBase);
96+
}
97+
98+
const auto comInWorldAdaptive = pinocchioCom_.centerOfMassInWorldFrame(basePose, jointPositionsAdaptive);
99+
const auto comInBaseAdaptive = pinocchioCom_.centerOfMassInBaseFrame(jointPositionsAdaptive);
100+
101+
// std::cout << "Pitch: " << pitchDeg << ", CoM default: " << comInWorldDefault.transpose()
102+
// << ", CoM adaptive: " << comInWorldAdaptive.transpose() << std::endl;
103+
std::cout << pitchDeg << ", " << comInWorldDefault.x() << ", " << comInWorldAdaptive.x() << std::endl;
104+
}
105+
}

0 commit comments

Comments
 (0)