From 308ec1dc5a296a8a56bfb1f152cc8ed3571709ba Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 25 Jun 2025 14:30:11 +0200 Subject: [PATCH 1/4] feat: restructure model kinematics to allow continuous joints --- CHANGELOG.md | 1 + source/controllers/README.md | 2 +- source/controllers/src/ControllerFactory.cpp | 3 +- .../test/tests/test_controller_factory.cpp | 8 +-- .../robot_model/include/robot_model/Model.hpp | 62 ++++++++++++++++-- source/robot_model/src/Model.cpp | 56 +++++++++++++--- .../test/tests/test_joint_types.cpp | 64 +++++++++++++++---- source/robot_model/test/tests/test_model.cpp | 4 +- 8 files changed, 164 insertions(+), 36 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5bb44623e..56841f543 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,7 @@ Release Versions - feat: add functionality to create a robot model from string (#200) - feat: update dependencies (#272) - feat: add joint types & limit some uses to supported types only (#243) +- feat: restructure joint getters and support continuous joints (#245, #246) ## 9.3.2 diff --git a/source/controllers/README.md b/source/controllers/README.md index 2a1f1f14c..435d505bc 100644 --- a/source/controllers/README.md +++ b/source/controllers/README.md @@ -150,7 +150,7 @@ auto ctrl = CartesianControllerFactory::create_controller(CONTROLLER_TYPE::IMPED auto robot = robot_model::Model("my_robot", "/path/to/robot.urdf"); // create some state variables -auto current_joints = JointState::Random(robot.get_robot_name(), robot.get_number_of_joints()); +auto current_joints = JointState::Random(robot.get_robot_name(), robot.get_configuration_dimension()); auto command_state = CartesianState::Random("command"); auto feedback_state = CartesianState::Random("feedback"); diff --git a/source/controllers/src/ControllerFactory.cpp b/source/controllers/src/ControllerFactory.cpp index 93c0dcfd9..0d4ee2af4 100644 --- a/source/controllers/src/ControllerFactory.cpp +++ b/source/controllers/src/ControllerFactory.cpp @@ -88,7 +88,8 @@ std::shared_ptr> ControllerFactory::create_c CONTROLLER_TYPE type, const std::list>& parameters, const robot_model::Model& robot_model ) { - auto ctrl = ControllerFactory::create_controller(type, parameters, robot_model.get_number_of_joints()); + auto ctrl = + ControllerFactory::create_controller(type, parameters, robot_model.get_configuration_dimension()); if (ctrl == nullptr) { throw exceptions::InvalidControllerException("Cannot assign robot model to this controller!"); } diff --git a/source/controllers/test/tests/test_controller_factory.cpp b/source/controllers/test/tests/test_controller_factory.cpp index 5d55eee9e..4ab77323d 100644 --- a/source/controllers/test/tests/test_controller_factory.cpp +++ b/source/controllers/test/tests/test_controller_factory.cpp @@ -180,7 +180,7 @@ TEST(ControllerFactoryTest, CreateControllerWithRobot) { auto command_state = CartesianState::Identity(robot.get_frames().back(), robot.get_base_frame()); auto feedback_state = CartesianState::Identity(robot.get_frames().back(), robot.get_base_frame()); - auto joint_state = JointState::Zero(robot_name, robot.get_number_of_joints()); + auto joint_state = JointState::Zero(robot_name, robot.get_configuration_dimension()); EXPECT_THROW( CartesianControllerFactory::create_controller(CONTROLLER_TYPE::NONE, robot), @@ -203,7 +203,7 @@ TEST(ControllerFactoryTest, CreateControllerWithRobot) { EXPECT_NO_THROW(joint_ctrl->compute_command(joint_state, joint_state)); EXPECT_EQ( joint_ctrl->get_parameter_value("stiffness").size(), - robot.get_number_of_joints() * robot.get_number_of_joints() + robot.get_configuration_dimension() * robot.get_configuration_dimension() ); } @@ -221,8 +221,8 @@ TEST(ControllerFactoryTest, CreateControllerWithRobotAndParams) { ASSERT_NE(ctrl, nullptr); EXPECT_EQ( ctrl->get_parameter_value("damping").size(), - robot.get_number_of_joints() * robot.get_number_of_joints() + robot.get_configuration_dimension() * robot.get_configuration_dimension() ); - EXPECT_EQ(ctrl->get_parameter_value("damping").sum(), 5.0 * robot.get_number_of_joints()); + EXPECT_EQ(ctrl->get_parameter_value("damping").sum(), 5.0 * robot.get_configuration_dimension()); } diff --git a/source/robot_model/include/robot_model/Model.hpp b/source/robot_model/include/robot_model/Model.hpp index 1211312d7..6fd145624 100644 --- a/source/robot_model/include/robot_model/Model.hpp +++ b/source/robot_model/include/robot_model/Model.hpp @@ -198,6 +198,10 @@ class Model { */ std::vector generate_joint_exclusion_list(); +protected: + Eigen::VectorXd + expand_joint_positions_to_full_configuration(const state_representation::JointPositions& joint_positions) const; + public: /** * @brief Construct with robot name and path to URDF file @@ -309,10 +313,22 @@ class Model { /** * @brief Getter of the number of joints - * @return the number of joints + * @return the number of joints (minus pinocchio's universe root joint and fixed joints) */ unsigned int get_number_of_joints() const; + /** + * @brief Getter of the degrees of freedom (dof) of the model + * @return the number of degrees of freedom (dof) of the model + */ + unsigned int get_dof() const; + + /** + * @brief Getter of the configuration dimension of the model + * @return the number of configuration dimensions of the model + */ + unsigned int get_configuration_dimension() const; + /** * @brief Getter of the joint frames from the model * @return the joint frames @@ -323,7 +339,7 @@ class Model { * @brief Getter of the types of the current model's joints * @return a vector of joint types */ - std::vector get_joint_types(); + std::vector get_joint_types() const; /** * @brief Getter of the frames from the model @@ -654,7 +670,15 @@ inline std::optional> Model::get_urdf_ } inline unsigned int Model::get_number_of_joints() const { - return this->robot_model_.nq;// ! todo: this is actually incorrect, that gives dofs + return this->robot_model_.njoints - 1; +} + +inline unsigned int Model::get_dof() const { + return this->robot_model_.nv; +} + +inline unsigned int Model::get_configuration_dimension() const { + return this->robot_model_.nq; } inline std::vector Model::get_joint_frames() const { @@ -729,7 +753,7 @@ struct JointTypeVisitor : public boost::static_visitor { } }; -inline std::vector Model::get_joint_types() { +inline std::vector Model::get_joint_types() const { std::vector joint_types; JointTypeVisitor classifier; for (int j = 1; j < this->robot_model_.njoints; ++j) {// skips the first joint (universe) @@ -739,4 +763,34 @@ inline std::vector Model::get_joint_types() { } return joint_types; } + +inline Eigen::VectorXd +Model::expand_joint_positions_to_full_configuration(const state_representation::JointPositions& joint_positions) const { + if (this->get_number_of_joints() == this->get_configuration_dimension()) { + return joint_positions.get_positions(); + } + Eigen::VectorXd q(this->get_configuration_dimension()); + const auto& joint_types = this->get_joint_types(); + const Eigen::VectorXd& positions = joint_positions.get_positions(); + std::size_t joint_input_idx = 0; + for (int i = 1; i < this->robot_model_.njoints; ++i) { + const auto& joint = this->robot_model_.joints[i]; + std::size_t idx_q = joint.idx_q(); + switch (joint_types[i - 1]) { + case JointType::CONTINUOUS: { + double angle = positions[joint_input_idx++]; + q[idx_q + 0] = std::cos(angle); + q[idx_q + 1] = std::sin(angle); + break; + } + default: { + for (int j = 0; j < joint.nq(); ++j) { + q[idx_q + j] = positions[joint_input_idx++]; + } + break; + } + } + } + return q; +} }// namespace robot_model diff --git a/source/robot_model/src/Model.cpp b/source/robot_model/src/Model.cpp index bc3711f5d..db82e8799 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -283,16 +283,50 @@ Model::compute_jacobian(const state_representation::JointPositions& joint_positi throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()); } // compute the Jacobian from the joint state - pinocchio::Data::Matrix6x J(6, this->get_number_of_joints()); + pinocchio::Data::Matrix6x J(6, this->get_configuration_dimension()); J.setZero(); - pinocchio::computeFrameJacobian( - this->robot_model_, this->robot_data_, joint_positions.data(), frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J - ); - // the model does not have any reference frame - return state_representation::Jacobian( - this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, J, - this->get_base_frame() - ); + if (this->get_configuration_dimension() != this->get_number_of_joints()) { + Eigen::VectorXd q = this->expand_joint_positions_to_full_configuration(joint_positions); + Eigen::MatrixXd P(this->robot_model_.nq, this->robot_model_.nv); + P.setZero(); + static const auto& joint_types = this->get_joint_types(); + for (int joint_input_idx = 0, i = 1; i < this->robot_model_.njoints; ++i) { + const auto& joint = this->robot_model_.joints[i]; + std::size_t idx_q = joint.idx_q(); + std::size_t idx_v = joint.idx_v(); + switch (joint_types[i - 1]) { + case JointType::CONTINUOUS: { + double angle = joint_positions.get_positions()[joint_input_idx++]; + P(idx_q, idx_v) = -std::sin(angle); + P(idx_q + 1, idx_v) = std::cos(angle); + break; + } + default: { + for (int j = 0; j < joint.nq(); ++j) { + P(idx_q + j, idx_v + j) = 1.0; + } + joint_input_idx += joint.nq(); + break; + } + } + } + Eigen::MatrixXd Jv = J * P; + pinocchio::computeFrameJacobian( + this->robot_model_, this->robot_data_, q, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, Jv + ); + return state_representation::Jacobian( + this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, Jv, + this->get_base_frame() + ); + } else {// the model does not have any reference frame + pinocchio::computeFrameJacobian( + this->robot_model_, this->robot_data_, joint_positions.data(), frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J + ); + return state_representation::Jacobian( + this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, J, + this->get_base_frame() + ); + } } state_representation::Jacobian @@ -379,8 +413,10 @@ std::vector Model::forward_kinematics( if (joint_positions.get_size() != this->get_number_of_joints()) { throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()); } + + Eigen::VectorXd q = this->expand_joint_positions_to_full_configuration(joint_positions); std::vector pose_vector; - pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, joint_positions.data()); + pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, q); for (unsigned int id : frame_ids) { if (id >= static_cast(this->robot_model_.nframes)) { throw exceptions::FrameNotFoundException(std::to_string(id)); diff --git a/source/robot_model/test/tests/test_joint_types.cpp b/source/robot_model/test/tests/test_joint_types.cpp index a8dc18ee5..50a0fbabb 100644 --- a/source/robot_model/test/tests/test_joint_types.cpp +++ b/source/robot_model/test/tests/test_joint_types.cpp @@ -1,6 +1,7 @@ #include "robot_model/Model.hpp" #include +#include #include #include @@ -21,24 +22,59 @@ class JointTypeTesting : public testing::Test { TEST_F(JointTypeTesting, CheckContinuousJoint) { // clang-format off - auto urdf = - "" - "" - "" - "" - "" - "" - "" - "" - "" - ""; + auto urdf = + "" + "" + "" + "" + "" + "" + "" + "" + "" + ""; // clang-format on + this->robot_ = std::make_unique("continuous", urdf); EXPECT_STREQ(this->robot_->get_robot_name().c_str(), "continuous"); auto joint_types = this->robot_->get_joint_types(); EXPECT_EQ(joint_types.size(), 1); EXPECT_EQ(joint_types.at(0), JointType::CONTINUOUS); - EXPECT_EQ(this->robot_->get_number_of_joints(), 2); + EXPECT_EQ(this->robot_->get_number_of_joints(), 1); + EXPECT_EQ(this->robot_->get_dof(), 1); + EXPECT_EQ(this->robot_->get_configuration_dimension(), 2); + + auto& data = this->robot_->get_pinocchio_data(); + + const double angle = M_PI / 3.0; + Eigen::VectorXd q = Eigen::VectorXd::Zero(this->robot_->get_number_of_joints()); + for (size_t i = 0; i < this->robot_->get_joint_types().size(); ++i) { + if (this->robot_->get_joint_types().at(i) == JointType::CONTINUOUS) { + q[i] = angle; + } + } + state_representation::JointState joint_state(this->robot_->get_robot_name(), this->robot_->get_joint_frames()); + joint_state.set_positions(q); + this->robot_->forward_kinematics(joint_state); + + const auto& M = data.oMi[1]; + Eigen::Matrix3d R_exp = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + EXPECT_TRUE(M.rotation().isApprox(R_exp, 1e-9)) << "Rotation should be a pure Rz(pi/3)"; + + Eigen::VectorXd v(1); + v << 1.0; + joint_state.set_velocities(v); + + auto twist = this->robot_->forward_velocity(joint_state); + EXPECT_EQ(twist.get_reference_frame(), this->robot_->get_base_frame()); + EXPECT_EQ(twist.get_name(), "base"); + + Eigen::Vector3d expected_angular(0.0, 0.0, 1.0); + EXPECT_TRUE(twist.get_angular_velocity().isApprox(expected_angular, 1e-9)) + << "Expected angular velocity [0, 0, 1], got: " << twist.get_angular_velocity().transpose(); + + EXPECT_TRUE(twist.get_linear_velocity().isZero(1e-9)) + << "Expected zero linear velocity, got: " << twist.get_linear_velocity().transpose(); } TEST_F(JointTypeTesting, CheckPrismaticJoint) { @@ -84,7 +120,7 @@ TEST_F(JointTypeTesting, CheckFloatingJoint) { // clang-format on this->robot_ = std::make_unique("floating", urdf); EXPECT_STREQ(this->robot_->get_robot_name().c_str(), "floating"); - EXPECT_EQ(this->robot_->get_number_of_joints(), 7); + EXPECT_EQ(this->robot_->get_number_of_joints(), 1); auto joint_types = this->robot_->get_joint_types(); EXPECT_EQ(joint_types.size(), 1); EXPECT_EQ(joint_types.at(0), JointType::FLOATING); @@ -128,7 +164,7 @@ TEST_F(JointTypeTesting, CheckPlanarJoint) { // clang-format on this->robot_ = std::make_unique("planar", urdf); EXPECT_STREQ(this->robot_->get_robot_name().c_str(), "planar"); - EXPECT_EQ(this->robot_->get_number_of_joints(), 4); + EXPECT_EQ(this->robot_->get_number_of_joints(), 1); auto joint_types = this->robot_->get_joint_types(); EXPECT_EQ(joint_types.size(), 1); EXPECT_EQ(joint_types.at(0), JointType::PLANAR); diff --git a/source/robot_model/test/tests/test_model.cpp b/source/robot_model/test/tests/test_model.cpp index 280317cb2..b7de8b865 100644 --- a/source/robot_model/test/tests/test_model.cpp +++ b/source/robot_model/test/tests/test_model.cpp @@ -80,7 +80,7 @@ TEST_F(RobotModelTest, TestEqualityConstructor) { } TEST_F(RobotModelTest, TestNumberOfJoints) { - EXPECT_EQ(franka->get_number_of_joints(), 7); + EXPECT_EQ(franka->get_configuration_dimension(), 7); } TEST_F(RobotModelTest, TestJacobianJointNames) { @@ -138,7 +138,7 @@ TEST_F(RobotModelTest, TestCreateURDFFromStringSuccess) { std::string urdf_string = strStream.str(); EXPECT_TRUE(Model::create_urdf_from_string(urdf_string, create_urdf_test_path)); Model fromCreatedUrdf = Model("fromCreatedUrdf", create_urdf_test_path); - EXPECT_EQ(fromCreatedUrdf.get_number_of_joints(), franka->get_number_of_joints()); + EXPECT_EQ(fromCreatedUrdf.get_configuration_dimension(), franka->get_configuration_dimension()); ASSERT_EQ(fromCreatedUrdf.get_frames().size(), franka->get_frames().size()); for (std::size_t frame = 0; frame < franka->get_frames().size(); ++frame) { EXPECT_EQ(fromCreatedUrdf.get_frames().at(frame), franka->get_frames().at(frame)); From f20d573f19aaf3d1cbbef5ca15bf835092419fa8 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 25 Jun 2025 14:32:03 +0200 Subject: [PATCH 2/4] feat: add directives indicating continuous joints are unlikely --- source/robot_model/src/Model.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/robot_model/src/Model.cpp b/source/robot_model/src/Model.cpp index db82e8799..b9e4aeb4d 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -285,7 +285,7 @@ Model::compute_jacobian(const state_representation::JointPositions& joint_positi // compute the Jacobian from the joint state pinocchio::Data::Matrix6x J(6, this->get_configuration_dimension()); J.setZero(); - if (this->get_configuration_dimension() != this->get_number_of_joints()) { + if (this->get_configuration_dimension() != this->get_number_of_joints()) [[unlikely]] { Eigen::VectorXd q = this->expand_joint_positions_to_full_configuration(joint_positions); Eigen::MatrixXd P(this->robot_model_.nq, this->robot_model_.nv); P.setZero(); @@ -318,7 +318,7 @@ Model::compute_jacobian(const state_representation::JointPositions& joint_positi this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, Jv, this->get_base_frame() ); - } else {// the model does not have any reference frame + } else [[likely]] {// the model does not have any reference frame pinocchio::computeFrameJacobian( this->robot_model_, this->robot_data_, joint_positions.data(), frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J ); From 979e7e7b6f1cc9b22ef3c7a607b543f18fac93eb Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Wed, 25 Jun 2025 14:35:16 +0200 Subject: [PATCH 3/4] revert: frank test changes --- source/robot_model/test/tests/test_model.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/robot_model/test/tests/test_model.cpp b/source/robot_model/test/tests/test_model.cpp index b7de8b865..280317cb2 100644 --- a/source/robot_model/test/tests/test_model.cpp +++ b/source/robot_model/test/tests/test_model.cpp @@ -80,7 +80,7 @@ TEST_F(RobotModelTest, TestEqualityConstructor) { } TEST_F(RobotModelTest, TestNumberOfJoints) { - EXPECT_EQ(franka->get_configuration_dimension(), 7); + EXPECT_EQ(franka->get_number_of_joints(), 7); } TEST_F(RobotModelTest, TestJacobianJointNames) { @@ -138,7 +138,7 @@ TEST_F(RobotModelTest, TestCreateURDFFromStringSuccess) { std::string urdf_string = strStream.str(); EXPECT_TRUE(Model::create_urdf_from_string(urdf_string, create_urdf_test_path)); Model fromCreatedUrdf = Model("fromCreatedUrdf", create_urdf_test_path); - EXPECT_EQ(fromCreatedUrdf.get_configuration_dimension(), franka->get_configuration_dimension()); + EXPECT_EQ(fromCreatedUrdf.get_number_of_joints(), franka->get_number_of_joints()); ASSERT_EQ(fromCreatedUrdf.get_frames().size(), franka->get_frames().size()); for (std::size_t frame = 0; frame < franka->get_frames().size(); ++frame) { EXPECT_EQ(fromCreatedUrdf.get_frames().at(frame), franka->get_frames().at(frame)); From 1e4b8f9f3643b8999bc799590de22aca7fc92991 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com> Date: Thu, 26 Jun 2025 17:45:25 +0200 Subject: [PATCH 4/4] fix: deprecated comment --- source/robot_model/src/Model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/robot_model/src/Model.cpp b/source/robot_model/src/Model.cpp index b9e4aeb4d..cef1e829c 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -318,7 +318,7 @@ Model::compute_jacobian(const state_representation::JointPositions& joint_positi this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, Jv, this->get_base_frame() ); - } else [[likely]] {// the model does not have any reference frame + } else [[likely]] { pinocchio::computeFrameJacobian( this->robot_model_, this->robot_data_, joint_positions.data(), frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J );