From 96b31258a6a8dee1c7d84c64f53353cc2b67bf77 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Wed, 3 Apr 2024 23:08:08 +0100 Subject: [PATCH] [state] Changed getRootDim API and fixed details in wb tests --- include/crocoddyl_msgs/conversions.h | 30 +++++++++++++------ .../whole_body_state_publisher.h | 2 +- .../whole_body_state_subscriber.h | 2 +- .../whole_body_trajectory_publisher.h | 2 +- .../whole_body_trajectory_subscriber.h | 2 +- src/crocoddyl_ros.cpp | 11 +++++-- unittest/test_whole_body_state.py | 18 +++++------ unittest/test_whole_body_trajectory.py | 17 ++++++----- 8 files changed, 50 insertions(+), 34 deletions(-) diff --git a/include/crocoddyl_msgs/conversions.h b/include/crocoddyl_msgs/conversions.h index 7b01230..cc8dbba 100644 --- a/include/crocoddyl_msgs/conversions.h +++ b/include/crocoddyl_msgs/conversions.h @@ -90,12 +90,24 @@ static inline std::size_t getRootJointId( } /** - * @brief Return the root dimension + * @brief Return the root nq dimension * - * @param return Root joint dimension + * @param return Root joint nq dimension */ template class JointCollectionTpl> -static inline std::size_t getRootDim( +static inline std::size_t getRootNq( + const pinocchio::ModelTpl &model) { + const std::size_t root_joint_id = getRootJointId(model); + return model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nq() : 0; +} + +/** + * @brief Return the root nv dimension + * + * @param return Root joint nv dimension + */ +template class JointCollectionTpl> +static inline std::size_t getRootNv( const pinocchio::ModelTpl &model) { const std::size_t root_joint_id = getRootJointId(model); return model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0; @@ -215,8 +227,8 @@ static inline void toMsg( " but received " + std::to_string(a.size())); } const std::size_t root_joint_id = getRootJointId(model); - const std::size_t nq_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nq() : 0; - const std::size_t nv_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0; + const std::size_t nq_root = getRootNq(model); + const std::size_t nv_root = getRootNv(model); const std::size_t njoints = model.nv - nv_root; if (tau.size() != static_cast(njoints) && tau.size() != 0) { throw std::invalid_argument("Expected tau to be 0 or " + @@ -689,8 +701,8 @@ static inline void fromReduced( const Eigen::Ref &qref, const std::vector &locked_joint_ids) { const std::size_t root_joint_id = getRootJointId(model); - const std::size_t nq_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nq() : 0; - const std::size_t nv_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0; + const std::size_t nq_root = getRootNq(model); + const std::size_t nv_root = getRootNv(model); const std::size_t njoints = model.nv - nv_root; const std::size_t njoints_reduced = reduced_model.nv - nv_root; if (q_out.size() != model.nq) { @@ -770,8 +782,8 @@ toReduced(const pinocchio::ModelTpl &model, const Eigen::Ref &v_in, const Eigen::Ref &tau_in) { const std::size_t root_joint_id = getRootJointId(model); - const std::size_t nq_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nq() : 0; - const std::size_t nv_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0; + const std::size_t nq_root = getRootNq(model); + const std::size_t nv_root = getRootNv(model); const std::size_t njoints = model.nv - nv_root; const std::size_t njoints_reduced = reduced_model.nv - nv_root; if (q_out.size() != reduced_model.nq) { diff --git a/include/crocoddyl_msgs/whole_body_state_publisher.h b/include/crocoddyl_msgs/whole_body_state_publisher.h index 240762e..c222bfc 100644 --- a/include/crocoddyl_msgs/whole_body_state_publisher.h +++ b/include/crocoddyl_msgs/whole_body_state_publisher.h @@ -183,7 +183,7 @@ class WholeBodyStateRosPublisher { } pinocchio::buildReducedModel(model_, joint_ids_, qref_, reduced_model_); // Initialize the vectors and dimensions - const std::size_t nv_root = getRootDim(model_); + const std::size_t nv_root = getRootNv(model_); qfull_ = Eigen::VectorXd::Zero(model_.nq); vfull_ = Eigen::VectorXd::Zero(model_.nv); ufull_ = Eigen::VectorXd::Zero(model_.nv - nv_root); diff --git a/include/crocoddyl_msgs/whole_body_state_subscriber.h b/include/crocoddyl_msgs/whole_body_state_subscriber.h index f3d2c15..5228a8b 100644 --- a/include/crocoddyl_msgs/whole_body_state_subscriber.h +++ b/include/crocoddyl_msgs/whole_body_state_subscriber.h @@ -221,7 +221,7 @@ class WholeBodyStateRosSubscriber { f_tmp_; void init(const std::vector &locked_joints = DEFAULT_VECTOR) { - const std::size_t nv_root = getRootDim(model_); + const std::size_t nv_root = getRootNv(model_); if (locked_joints.size() != 0) { // Check the size of the reference configuration if (qref_.size() != model_.nq) { diff --git a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h index ae13495..e698479 100644 --- a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h +++ b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h @@ -253,7 +253,7 @@ class WholeBodyTrajectoryRosPublisher { // Initialize the vectors and dimensions const std::size_t root_joint_id = getRootJointId(model_); - const std::size_t nv_root = getRootDim(model_); + const std::size_t nv_root = getRootNv(model_); qfull_ = Eigen::VectorXd::Zero(model_.nq); vfull_ = Eigen::VectorXd::Zero(model_.nv); ufull_ = Eigen::VectorXd::Zero(model_.nv - nv_root); diff --git a/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h b/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h index 5f27a63..39ef0a1 100644 --- a/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h +++ b/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h @@ -254,7 +254,7 @@ class WholeBodyTrajectoryRosSubscriber { spinner_.start(); #endif - const std::size_t nv_root = getRootDim(model_); + const std::size_t nv_root = getRootNv(model_); if (locked_joints.size() != 0) { // Check the size of the reference configuration if (qref_.size() != model_.nq) { diff --git a/src/crocoddyl_ros.cpp b/src/crocoddyl_ros.cpp index 38a7618..dc86d50 100644 --- a/src/crocoddyl_ros.cpp +++ b/src/crocoddyl_ros.cpp @@ -250,10 +250,15 @@ PYBIND11_MODULE(crocoddyl_ros, m) { ":param model: Pinocchio model\n" ":return root joint id"); - m.def("getRootDim", &getRootDim<0, pinocchio::JointCollectionDefaultTpl>, - "Return the root joint dimension.\n\n" + m.def("getRootNq", &getRootNq<0, pinocchio::JointCollectionDefaultTpl>, + "Return the root joint nq dimension.\n\n" ":param model: Pinocchio model\n" - ":return root joint dimension"); + ":return root joint nq dimension"); + + m.def("getRootNv", &getRootNv<0, pinocchio::JointCollectionDefaultTpl>, + "Return the root joint nv dimension.\n\n" + ":param model: Pinocchio model\n" + ":return root joint nv dimension"); m.def( "fromReduced", diff --git a/unittest/test_whole_body_state.py b/unittest/test_whole_body_state.py index 6872a08..e396828 100755 --- a/unittest/test_whole_body_state.py +++ b/unittest/test_whole_body_state.py @@ -29,7 +29,7 @@ WholeBodyStateRosPublisher, WholeBodyStateRosSubscriber, toReduced, - getRootDim, + getRootNv, ) @@ -89,7 +89,7 @@ def test_publisher_without_contact(self): pub = WholeBodyStateRosPublisher(self.MODEL, "whole_body_state_without_contact") time.sleep(1) # publish whole-body state messages - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) q = pinocchio.randomConfiguration(self.MODEL) v = np.random.rand(self.MODEL.nv) tau = np.random.rand(self.MODEL.nv - nv_root) @@ -111,7 +111,7 @@ def test_communication(self): pub = WholeBodyStateRosPublisher(self.MODEL, "whole_body_state") time.sleep(1) # publish whole-body state messages - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) q = pinocchio.randomConfiguration(self.MODEL) v = np.random.rand(self.MODEL.nv) tau = np.random.rand(self.MODEL.nv - nv_root) @@ -158,19 +158,17 @@ def test_communication(self): ) def test_communication_with_reduced_model(self): - # locked_joints = ["larm_elbow_joint", "rarm_elbow_joint"] - locked_joints = ["wrist2_joint"] qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in locked_joints], qref + self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref ) sub = WholeBodyStateRosSubscriber( - self.MODEL, locked_joints, qref, "reduced_whole_body_state" + self.MODEL, self.LOCKED_JOINTS, qref, "reduced_whole_body_state" ) - pub = WholeBodyStateRosPublisher(self.MODEL, locked_joints, qref, "reduced_whole_body_state") + pub = WholeBodyStateRosPublisher(self.MODEL, self.LOCKED_JOINTS, qref, "reduced_whole_body_state") time.sleep(1) # publish whole-body state messages - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) q = pinocchio.randomConfiguration(self.MODEL) v = np.random.rand(self.MODEL.nv) tau = np.random.rand(self.MODEL.nv - nv_root) @@ -229,7 +227,7 @@ def test_communication_with_non_locked_joints(self): pub = WholeBodyStateRosPublisher(self.MODEL, locked_joints, qref, "non_locked_whole_body_state") time.sleep(1) # publish whole-body state messages - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) q = pinocchio.randomConfiguration(self.MODEL) v = np.random.rand(self.MODEL.nv) tau = np.random.rand(self.MODEL.nv - nv_root) diff --git a/unittest/test_whole_body_trajectory.py b/unittest/test_whole_body_trajectory.py index 3ae9aec..5ca875c 100755 --- a/unittest/test_whole_body_trajectory.py +++ b/unittest/test_whole_body_trajectory.py @@ -29,7 +29,7 @@ WholeBodyTrajectoryRosPublisher, WholeBodyTrajectoryRosSubscriber, toReduced, - getRootDim, + getRootNv, ) @@ -103,7 +103,7 @@ def test_publisher_without_contact(self): # publish whole-body trajectory messages N = len(self.ts) xs = [] - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) for _ in range(N): q = pinocchio.randomConfiguration(self.MODEL) q[:3] = np.random.rand(3) @@ -135,7 +135,7 @@ def test_communication(self): q[:3] = np.random.rand(3) v = np.random.rand(self.MODEL.nv) xs.append(np.hstack([q, v])) - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) us = [np.random.rand(self.MODEL.nv - nv_root) for _ in range(N)] while True: pub.publish(self.ts, xs, us, self.ps, self.pds, self.fs, self.ss) @@ -200,7 +200,7 @@ def test_communication_with_reduced_model(self): # publish whole-body trajectory messages N = len(self.ts) xs, us = [], [] - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) for _ in range(N): q = pinocchio.randomConfiguration(self.MODEL) q[:3] = np.random.rand(3) @@ -258,21 +258,22 @@ def test_communication_with_reduced_model(self): ) def test_communication_with_non_locked_joints(self): + locked_joints = [] qref = pinocchio.randomConfiguration(self.MODEL) reduced_model = pinocchio.buildReducedModel( - self.MODEL, [self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS], qref + self.MODEL, [self.MODEL.getJointId(name) for name in locked_joints], qref ) sub = WholeBodyTrajectoryRosSubscriber( - self.MODEL, self.LOCKED_JOINTS, qref, "non_locked_whole_body_trajectory" + self.MODEL, locked_joints, qref, "non_locked_whole_body_trajectory" ) pub = WholeBodyTrajectoryRosPublisher( - self.MODEL, self.LOCKED_JOINTS, qref, "non_locked_whole_body_trajectory" + self.MODEL, locked_joints, qref, "non_locked_whole_body_trajectory" ) time.sleep(1) # publish whole-body trajectory messages N = len(self.ts) xs, us = [], [] - nv_root = getRootDim(self.MODEL) + nv_root = getRootNv(self.MODEL) for _ in range(N): q = pinocchio.randomConfiguration(self.MODEL) q[:3] = np.random.rand(3)