diff --git a/CHANGELOG.md b/CHANGELOG.md index e9ecdf459..d2c381d3b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,7 @@ Release Versions - feat: expose pinocchio::Data through getter (#238) - feat: add functionality to create a robot model from string (#200) - feat: add joint types & limit some uses to supported types only (#243) +- feat: restructure joint getters and support continuous joints (#245, #246) ## 9.2.0 diff --git a/source/controllers/README.md b/source/controllers/README.md index 319ff4acd..b14e97d0f 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 c9d476b3f..763daf05e 100644 --- a/source/robot_model/include/robot_model/Model.hpp +++ b/source/robot_model/include/robot_model/Model.hpp @@ -196,6 +196,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 @@ -307,10 +311,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 @@ -321,7 +337,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 @@ -651,7 +667,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 { @@ -726,7 +750,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) @@ -736,4 +760,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 0ee2fae62..23d361ddd 100644 --- a/source/robot_model/src/Model.cpp +++ b/source/robot_model/src/Model.cpp @@ -277,16 +277,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()) [[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(); + 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 [[likely]] { + 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 @@ -373,8 +407,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);