From 9ce070f6d602d9745552291c140a131415282a80 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 20 Dec 2024 15:42:31 +0100 Subject: [PATCH 1/2] Reduced model is now built outside robot_handler --- benchmark/talos.cpp | 33 +++++++++++--- bindings/expose-robot-handler.cpp | 9 ++-- examples/go2_fulldynamics.py | 2 +- examples/go2_kinodynamics.py | 6 +-- examples/talos_centroidal.py | 19 ++------ examples/talos_fulldynamics.py | 29 +++++-------- examples/talos_kinodynamics.py | 23 +++------- examples/utils.py | 13 ++++++ include/simple-mpc/robot-handler.hpp | 29 +------------ src/robot-handler.cpp | 65 ++++------------------------ tests/robot_handler.cpp | 53 +++++++++++------------ tests/test_utils.cpp | 33 +++++++++++--- 12 files changed, 130 insertions(+), 184 deletions(-) diff --git a/benchmark/talos.cpp b/benchmark/talos.cpp index 446ad4b..bbaa6ed 100644 --- a/benchmark/talos.cpp +++ b/benchmark/talos.cpp @@ -2,6 +2,7 @@ #include "simple-mpc/mpc.hpp" #include "simple-mpc/ocp-handler.hpp" #include "simple-mpc/robot-handler.hpp" +#include #include #include #include @@ -19,13 +20,13 @@ using simple_mpc::OCPHandler; int main() { // Load pinocchio model from example robot data - pinocchio::Model model; + pinocchio::Model modelFull; std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf"; std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf"; - pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model); - pinocchio::srdf::loadReferenceConfigurations(model, srdf_path, false); - pinocchio::srdf::loadRotorParameters(model, srdf_path, false); + pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), modelFull); + pinocchio::srdf::loadReferenceConfigurations(modelFull, srdf_path, false); + pinocchio::srdf::loadRotorParameters(modelFull, srdf_path, false); // Lock joint list const std::vector controlled_joints_names{ @@ -36,7 +37,8 @@ int main() "arm_right_1_joint", "arm_right_2_joint", "arm_right_3_joint", "arm_right_4_joint", }; - std::vector locked_joints_names{model.names}; + std::vector locked_joints_names{modelFull.names}; + std::vector controlled_joints_ids; locked_joints_names.erase( std::remove_if( locked_joints_names.begin(), locked_joints_names.end(), @@ -46,9 +48,28 @@ int main() }), locked_joints_names.end()); + // Construct controlled and locked joints ids list + pinocchio::Model model; + std::vector locked_joint_ids; + for (size_t i = 1; i < modelFull.names.size(); i++) + { + const std::string joint_name = modelFull.names.at(i); + if (count(locked_joints_names.begin(), locked_joints_names.end(), joint_name) == 0) + { + controlled_joints_ids.push_back(modelFull.getJointId(joint_name)); + } + else + { + locked_joint_ids.push_back(modelFull.getJointId(joint_name)); + } + } + + // Build reduced model with locked joints + pinocchio::buildReducedModel(modelFull, locked_joint_ids, modelFull.referenceConfigurations["half_sitting"], model); + // Actually create handler std::string base_joint = "root_joint"; - RobotModelHandler model_handler(model, "half_sitting", base_joint, locked_joints_names); + RobotModelHandler model_handler(model, "half_sitting", base_joint); // Add feet model_handler.addFoot( diff --git a/bindings/expose-robot-handler.cpp b/bindings/expose-robot-handler.cpp index 2c77edf..f9984fb 100644 --- a/bindings/expose-robot-handler.cpp +++ b/bindings/expose-robot-handler.cpp @@ -21,12 +21,10 @@ namespace simple_mpc void exposeHandler() { bp::class_( - "RobotModelHandler", - bp::init &>( - bp::args("self", "model", "reference_configuration_name", "base_frame_name", "locked_joint_names"))) + "RobotModelHandler", bp::init( + bp::args("self", "model", "reference_configuration_name", "base_frame_name"))) .def("addFoot", &RobotModelHandler::addFoot) .def("difference", &RobotModelHandler::difference) - .def("shapeState", &RobotModelHandler::shapeState) .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId) .def("getReferenceState", &RobotModelHandler::getReferenceState) .def("getFootNb", &RobotModelHandler::getFootNb) @@ -37,8 +35,7 @@ namespace simple_mpc .def("getFootId", &RobotModelHandler::getFootId) .def("getRefFootId", &RobotModelHandler::getRefFootId) .def("getMass", &RobotModelHandler::getMass) - .def("getModel", &RobotModelHandler::getModel, bp::return_internal_reference<>()) - .def("getCompleteModel", &RobotModelHandler::getCompleteModel, bp::return_internal_reference<>()); + .def("getModel", &RobotModelHandler::getModel, bp::return_internal_reference<>()); ENABLE_SPECIFIC_MATRIX_TYPE(RobotDataHandler::CentroidalStateVector); diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index 97378c8..7470011 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -14,7 +14,7 @@ robot_wrapper = erd.load('go2') # Create Model and Data handler -model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name, []) +model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name) model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1]))) model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1]))) model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1]))) diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py index 59b8d6b..d2352d9 100644 --- a/examples/go2_kinodynamics.py +++ b/examples/go2_kinodynamics.py @@ -14,7 +14,7 @@ robot_wrapper = erd.load('go2') # Create Model and Data handler -model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name, []) +model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name) model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1]))) model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1]))) model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1]))) @@ -225,8 +225,8 @@ com_measured.append(mpc.getDataHandler().getData().com[0].copy()) L_measured.append(mpc.getDataHandler().getData().hg.angular.copy()) - a0 = mpc.getStateDerivative(0)[nv:] - a1 = mpc.getStateDerivative(1)[nv:] + a0 = mpc.getStateDerivative(0)[nv:].copy() + a1 = mpc.getStateDerivative(1)[nv:].copy() a0[6:] = mpc.us[0][nk * force_size :] a1[6:] = mpc.us[1][nk * force_size :] diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py index aacfe31..a98a754 100644 --- a/examples/talos_centroidal.py +++ b/examples/talos_centroidal.py @@ -3,29 +3,18 @@ import example_robot_data as erd from bullet_robot import BulletRobot from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC, IKIDSolver +from utils import loadTalos import time # RobotWrapper URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf" base_joint_name ="root_joint" -robot_wrapper = erd.load('talos') - reference_configuration_name = "half_sitting" -locked_joints = [ - 'arm_left_5_joint', - 'arm_left_6_joint', - 'arm_left_7_joint', - 'gripper_left_joint', - 'arm_right_5_joint', - 'arm_right_6_joint', - 'arm_right_7_joint', - 'gripper_right_joint', - 'head_1_joint', - 'head_2_joint' -] + +rmodelComplete, rmodel, qComplete, q0 = loadTalos() # Create Model and Data handler -model_handler = RobotModelHandler(robot_wrapper.model, reference_configuration_name, base_joint_name, locked_joints) +model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name) model_handler.addFoot("left_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1]))) model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1]))) data_handler = RobotDataHandler(model_handler) diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py index e65400a..07dbf01 100644 --- a/examples/talos_fulldynamics.py +++ b/examples/talos_fulldynamics.py @@ -7,36 +7,28 @@ import numpy as np import time from bullet_robot import BulletRobot -import example_robot_data as erd import pinocchio as pin from simple_mpc import MPC, FullDynamicsOCP, RobotModelHandler, RobotDataHandler +from utils import loadTalos +import example_robot_data as erd # ####### CONFIGURATION ############ # RobotWrapper URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf" base_joint_name ="root_joint" -robot_wrapper = erd.load('talos') - reference_configuration_name = "half_sitting" -locked_joints = [ - 'arm_left_5_joint', - 'arm_left_6_joint', - 'arm_left_7_joint', - 'gripper_left_joint', - 'arm_right_5_joint', - 'arm_right_6_joint', - 'arm_right_7_joint', - 'gripper_right_joint', - 'head_1_joint', - 'head_2_joint' -] + +rmodelComplete, rmodel, qComplete, q0 = loadTalos() # Create Model and Data handler -model_handler = RobotModelHandler(robot_wrapper.model, reference_configuration_name, base_joint_name, locked_joints) +model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name) model_handler.addFoot("left_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1]))) model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1]))) data_handler = RobotDataHandler(model_handler) +controlled_joints = rmodel.names[1:].tolist() +controlled_ids = [rmodelComplete.getJointId(name_joint) for name_joint in controlled_joints[1:]] + nq = model_handler.getModel().nq nv = model_handler.getModel().nv nu = nv - 6 @@ -150,7 +142,7 @@ device.changeCamera(1.0, 90, -5, [1.5, 0, 1]) q_meas, v_meas = device.measureState() -x_measured = np.concatenate([q_meas, v_meas]) +x_measured = np.concatenate((q_meas, v_meas)) land_LF = -1 land_RF = -1 @@ -181,6 +173,7 @@ str(land_LF), ) start = time.time() + mpc.iterate(x_measured) end = time.time() print("MPC iterate = " + str(end - start)) @@ -191,7 +184,7 @@ for j in range(10): q_meas, v_meas = device.measureState() - x_measured = np.concatenate([q_meas, v_meas]) + x_measured = np.concatenate((q_meas, v_meas)) current_torque = mpc.us[0] - mpc.Ks[0] @ model_handler.difference(x_measured, mpc.xs[0]) device.execute(current_torque) diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py index aacf38b..f18ef0c 100644 --- a/examples/talos_kinodynamics.py +++ b/examples/talos_kinodynamics.py @@ -3,29 +3,18 @@ import pinocchio as pin from bullet_robot import BulletRobot import time +from utils import loadTalos from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver # RobotWrapper URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf" base_joint_name ="root_joint" -robot_wrapper = erd.load('talos') - reference_configuration_name = "half_sitting" -locked_joints = [ - 'arm_left_5_joint', - 'arm_left_6_joint', - 'arm_left_7_joint', - 'gripper_left_joint', - 'arm_right_5_joint', - 'arm_right_6_joint', - 'arm_right_7_joint', - 'gripper_right_joint', - 'head_1_joint', - 'head_2_joint' -] + +rmodelComplete, rmodel, qComplete, q0 = loadTalos() # Create Model and Data handler -model_handler = RobotModelHandler(robot_wrapper.model, reference_configuration_name, base_joint_name, locked_joints) +model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name) model_handler.addFoot("left_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1]))) model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1]))) data_handler = RobotDataHandler(model_handler) @@ -211,8 +200,8 @@ end = time.time() print("MPC iterate = " + str(end - start)) - a0 = mpc.getStateDerivative(0)[nv:] - a1 = mpc.getStateDerivative(1)[nv:] + a0 = mpc.getStateDerivative(0)[nv:].copy() + a1 = mpc.getStateDerivative(1)[nv:].copy() a0[6:] = mpc.us[0][nk * force_size :] a1[6:] = mpc.us[1][nk * force_size :] forces0 = mpc.us[0][: nk * force_size] diff --git a/examples/utils.py b/examples/utils.py index b838004..3aba793 100644 --- a/examples/utils.py +++ b/examples/utils.py @@ -1,9 +1,22 @@ import numpy as np +import pinocchio as pin +import example_robot_data import os CURRENT_DIRECTORY = os.getcwd() DEFAULT_SAVE_DIR = CURRENT_DIRECTORY + '/tmp' +def loadTalos(): + robotComplete = example_robot_data.load("talos") + qComplete = robotComplete.model.referenceConfigurations["half_sitting"] + + locked_joints = [20,21,22,23,28,29,30,31] + locked_joints += [32, 33] + robot = robotComplete.buildReducedRobot(locked_joints, qComplete) + rmodel: pin.Model = robot.model + q0 = rmodel.referenceConfigurations["half_sitting"] + + return robotComplete.model, rmodel, qComplete, q0 def save_trajectory( xs, diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp index c6b0aa1..6974eca 100644 --- a/include/simple-mpc/robot-handler.hpp +++ b/include/simple-mpc/robot-handler.hpp @@ -29,12 +29,7 @@ namespace simple_mpc { private: /** - * @brief Robot model with all joints unlocked - */ - Model model_full_; - - /** - * @brief Reduced model to be used by ocp + * @brief Model to be used by ocp */ Model model_; @@ -82,14 +77,9 @@ namespace simple_mpc * @param feet_names Name of the frames corresponding to the feet (e.g. can be * used for contact with the ground) * @param reference_configuration_name Reference configuration to use - * @param locked_joint_names List of joints to lock (values will be fixed at - * the reference configuration) */ RobotModelHandler( - const Model & model, - const std::string & reference_configuration_name, - const std::string & base_frame_name, - const std::vector & locked_joint_names = {}); + const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name); /** * @brief @@ -113,16 +103,6 @@ namespace simple_mpc */ Eigen::VectorXd difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const; - /** - * @brief Compute reduced state from measures by concatenating q,v of the - * reduced model. - * - * @param q Configuration vector of the full model - * @param v Velocity vector of the full model - * @return const Eigen::VectorXd State vector of the reduced model. - */ - Eigen::VectorXd shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const; - // Const getters ConstVectorRef getReferenceState() const { @@ -174,11 +154,6 @@ namespace simple_mpc { return model_; } - - const Model & getCompleteModel() const - { - return model_full_; - } }; class RobotDataHandler diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp index d66774b..a2677e9 100644 --- a/src/robot-handler.cpp +++ b/src/robot-handler.cpp @@ -11,37 +11,19 @@ namespace simple_mpc { RobotModelHandler::RobotModelHandler( - const Model & model, - const std::string & reference_configuration_name, - const std::string & base_frame_name, - const std::vector & locked_joint_names) - : model_full_(model) + const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name) + : model_(model) { - // Construct controlled and locked joints ids list - std::vector locked_joint_ids; - for (size_t i = 1; i < model_full_.names.size(); i++) - { - const std::string joint_name = model_full_.names.at(i); - if (count(locked_joint_names.begin(), locked_joint_names.end(), joint_name) == 0) - { - controlled_joints_ids_.push_back(model_full_.getJointId(joint_name)); - } - else - { - locked_joint_ids.push_back(model_full_.getJointId(joint_name)); - } - } - - // Build reduced model with locked joints - buildReducedModel( - model_full_, locked_joint_ids, model_full_.referenceConfigurations[reference_configuration_name], model_); + // Controlled joints index + for (auto joint_name : model_.names) + controlled_joints_ids_.push_back(model.getJointId(joint_name)); // Root frame id base_id_ = model_.getFrameId(base_frame_name); // Set reference state - reference_state_ = shapeState( - model_full_.referenceConfigurations[reference_configuration_name], Eigen::VectorXd::Zero(model_full_.nv)); + reference_state_.resize(model_.nq + model_.nv); + reference_state_ << model_.referenceConfigurations[reference_configuration_name], Eigen::VectorXd::Zero(model_.nv); // Mass mass_ = pinocchio::computeTotalMass(model_); @@ -66,37 +48,6 @@ namespace simple_mpc return frame_id; } - Eigen::VectorXd RobotModelHandler::shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const - { - const long nq_full = model_full_.nq; - const long nv_full = model_full_.nv; - const long nq = model_.nq; - const long nv = model_.nv; - const long nx = nq + nv; - Eigen::VectorXd x(nx); - - assert(nq_full == q.size() && "Configuration vector has wrong size."); - assert(nv_full == v.size() && "Velocity vector has wrong size."); - - // Copy each controlled joint to state vector - int iq = 0; - int iv = (int)nq; - for (unsigned long jointId : controlled_joints_ids_) - { - const long j_idx_q = model_full_.idx_qs[jointId]; - const long j_idx_v = model_full_.idx_vs[jointId]; - const long j_nq = model_full_.nqs[jointId]; - const long j_nv = model_full_.nvs[jointId]; - - x.segment(iq, j_nq) = q.segment(j_idx_q, j_nq); - x.segment(iv, j_nv) = v.segment(j_idx_v, j_nv); - - iq += j_nq; - iv += j_nv; - } - return x; - } - Eigen::VectorXd RobotModelHandler::difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const { const size_t nq = (size_t)model_.nq; @@ -119,7 +70,7 @@ namespace simple_mpc std::vector joint_names; for (JointIndex id : controlled_joints_ids_) { - joint_names.push_back(model_full_.names.at(id)); + joint_names.push_back(model_.names.at(id)); } return joint_names; } diff --git a/tests/robot_handler.cpp b/tests/robot_handler.cpp index bb1bef9..f10b3ae 100644 --- a/tests/robot_handler.cpp +++ b/tests/robot_handler.cpp @@ -26,7 +26,6 @@ BOOST_AUTO_TEST_CASE(model_handler) Model model; const std::string base_frame = "root_joint"; const std::string default_conf_name = "straight_standing"; - const std::vector locked_joints{"FR_HFE", "FL_HFE"}; // Lock two random joint for testing const std::vector feet_names{"FR_FOOT", "FL_FOOT", "HR_FOOT", "HL_FOOT"}; const std::vector feet_refs{ SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, -0.1, 0.)), @@ -44,7 +43,7 @@ BOOST_AUTO_TEST_CASE(model_handler) pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model); srdf::loadReferenceConfigurations(model, srdf_path, false); - RobotModelHandler model_handler(model, default_conf_name, base_frame, locked_joints); + RobotModelHandler model_handler(model, default_conf_name, base_frame); // Add feet for (size_t i = 0; i < feet_names.size(); i++) @@ -57,9 +56,8 @@ BOOST_AUTO_TEST_CASE(model_handler) /*********/ // Model { - BOOST_CHECK(model_handler.getCompleteModel() == model); - BOOST_CHECK_EQUAL(model_handler.getModel().nq, 17); - BOOST_CHECK_EQUAL(model_handler.getModel().nv, 16); + BOOST_CHECK_EQUAL(model_handler.getModel().nq, 19); + BOOST_CHECK_EQUAL(model_handler.getModel().nv, 18); } // Base frame @@ -86,11 +84,11 @@ BOOST_AUTO_TEST_CASE(model_handler) // State { - const size_t nq_red = 17; + const int nq_red = 19; Eigen::Vector q = pinocchio::randomConfiguration(model); q.head<3>() = Eigen::Vector3d::Random(); Eigen::Vector v = Eigen::Vector::Random(); - Eigen::Vector x = Eigen::Vector::Random(); + Eigen::Vector x = Eigen::Vector::Random(); bool is_reference_state = false; for (int n = 0; n < 2; n++) // First time with random data, second with reference state @@ -98,17 +96,14 @@ BOOST_AUTO_TEST_CASE(model_handler) // State vector without locked joints for (int i = 1; i < model_handler.getModel().njoints; i++) { - const std::string & joint_name = model_handler.getModel().names[i]; + const std::string & joint_name = model_handler.getModel().names[(size_t)i]; const JointModel & joint_full = model.joints[model.getJointId(joint_name)]; - const JointModel & joint_red = model_handler.getModel().joints[i]; + const JointModel & joint_red = model_handler.getModel().joints[(size_t)i]; x.segment(joint_red.idx_q(), joint_red.nq()) = q.segment(joint_full.idx_q(), joint_full.nq()); x.segment(nq_red + joint_red.idx_v(), joint_red.nv()) = v.segment(joint_full.idx_v(), joint_full.nv()); } - // Test shape state - BOOST_CHECK(model_handler.shapeState(q, v).isApprox(x)); - // Test reference state if (is_reference_state) { @@ -124,28 +119,28 @@ BOOST_AUTO_TEST_CASE(model_handler) // Difference { - Eigen::Vector q1 = pinocchio::randomConfiguration(model_handler.getModel()); + Eigen::Vector q1 = pinocchio::randomConfiguration(model_handler.getModel()); q1.head<3>() = Eigen::Vector3d::Random(); - Eigen::Vector q2 = pinocchio::randomConfiguration(model_handler.getModel()); + Eigen::Vector q2 = pinocchio::randomConfiguration(model_handler.getModel()); q2.head<3>() = Eigen::Vector3d::Random(); - const Eigen::Vector v1 = Eigen::Vector::Random(); - const Eigen::Vector v2 = Eigen::Vector::Random(); + const Eigen::Vector v1 = Eigen::Vector::Random(); + const Eigen::Vector v2 = Eigen::Vector::Random(); - Eigen::Vector x1, x2; - x1.head<17>() = q1; - x1.tail<16>() = v1; - x2.head<17>() = q2; - x2.tail<16>() = v2; + Eigen::Vector x1, x2; + x1.head<19>() = q1; + x1.tail<18>() = v1; + x2.head<19>() = q2; + x2.tail<18>() = v2; - const Eigen::Vector diff = model_handler.difference(x1, x2); + const Eigen::Vector diff = model_handler.difference(x1, x2); - const Eigen::Vector dq; + const Eigen::Vector dq; pinocchio::difference(model_handler.getModel(), q1, q2, dq); - const Eigen::Vector dv = v2 - v1; + const Eigen::Vector dv = v2 - v1; - BOOST_CHECK(dq.isApprox(diff.head<16>())); - BOOST_CHECK(dv.isApprox(diff.tail<16>())); + BOOST_CHECK(dq.isApprox(diff.head<18>())); + BOOST_CHECK(dv.isApprox(diff.tail<18>())); } // Mass @@ -170,10 +165,12 @@ BOOST_AUTO_TEST_CASE(data_handler) const Model & model = model_handler.getModel(); Data data(model); - Eigen::Vector q = pinocchio::randomConfiguration(model_handler.getCompleteModel()); + Eigen::Vector q = pinocchio::randomConfiguration(model_handler.getModel()); q.head<3>() = Eigen::Vector3d::Random(); const Eigen::Vector v = Eigen::Vector::Random(); - const Eigen::Vector x = model_handler.shapeState(q, v); + Eigen::Vector x; + x.head<19>() = q; + x.tail<18>() = v; /*********/ /* Tests */ diff --git a/tests/test_utils.cpp b/tests/test_utils.cpp index 8e43997..b34370f 100644 --- a/tests/test_utils.cpp +++ b/tests/test_utils.cpp @@ -4,6 +4,7 @@ #include "simple-mpc/fulldynamics.hpp" #include "simple-mpc/kinodynamics.hpp" #include "simple-mpc/robot-handler.hpp" +#include #include #include #include @@ -13,13 +14,13 @@ using namespace simple_mpc; RobotModelHandler getTalosModelHandler() { // Load pinocchio model from example robot data - Model model; + Model modelFull; std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf"; std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf"; - pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model); - srdf::loadReferenceConfigurations(model, srdf_path, false); - srdf::loadRotorParameters(model, srdf_path, false); + pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), modelFull); + srdf::loadReferenceConfigurations(modelFull, srdf_path, false); + srdf::loadRotorParameters(modelFull, srdf_path, false); // Lock joint list const std::vector controlled_joints_names{ @@ -30,7 +31,7 @@ RobotModelHandler getTalosModelHandler() "arm_right_1_joint", "arm_right_2_joint", "arm_right_3_joint", "arm_right_4_joint", }; - std::vector locked_joints_names{model.names}; + std::vector locked_joints_names{modelFull.names}; locked_joints_names.erase( std::remove_if( locked_joints_names.begin(), locked_joints_names.end(), @@ -40,9 +41,29 @@ RobotModelHandler getTalosModelHandler() }), locked_joints_names.end()); + // Construct controlled and locked joints ids list + pinocchio::Model model; + std::vector locked_joint_ids; + std::vector controlled_joints_ids; + for (size_t i = 1; i < modelFull.names.size(); i++) + { + const std::string joint_name = modelFull.names.at(i); + if (count(locked_joints_names.begin(), locked_joints_names.end(), joint_name) == 0) + { + controlled_joints_ids.push_back(modelFull.getJointId(joint_name)); + } + else + { + locked_joint_ids.push_back(modelFull.getJointId(joint_name)); + } + } + + // Build reduced model with locked joints + pinocchio::buildReducedModel(modelFull, locked_joint_ids, modelFull.referenceConfigurations["half_sitting"], model); + // Actually create handler std::string base_joint = "root_joint"; - RobotModelHandler model_handler(model, "half_sitting", base_joint, locked_joints_names); + RobotModelHandler model_handler(model, "half_sitting", base_joint); // Add feet model_handler.addFoot( From b4a3315d7b68836b98d35ac8c648b6cda3ee9ed7 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Mon, 6 Jan 2025 11:32:42 +0100 Subject: [PATCH 2/2] Remove joint ids and joint names --- bindings/expose-robot-handler.cpp | 1 - examples/utils.py | 9 +++++++-- include/simple-mpc/robot-handler.hpp | 7 ------- src/robot-handler.cpp | 15 --------------- 4 files changed, 7 insertions(+), 25 deletions(-) diff --git a/bindings/expose-robot-handler.cpp b/bindings/expose-robot-handler.cpp index f9984fb..52fb6f9 100644 --- a/bindings/expose-robot-handler.cpp +++ b/bindings/expose-robot-handler.cpp @@ -31,7 +31,6 @@ namespace simple_mpc .def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>()) .def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>()) .def("getFeetNames", &RobotModelHandler::getFeetNames, bp::return_internal_reference<>()) - .def("getControlledJointNames", &RobotModelHandler::getControlledJointNames) .def("getFootId", &RobotModelHandler::getFootId) .def("getRefFootId", &RobotModelHandler::getRefFootId) .def("getMass", &RobotModelHandler::getMass) diff --git a/examples/utils.py b/examples/utils.py index 3aba793..dbc1020 100644 --- a/examples/utils.py +++ b/examples/utils.py @@ -10,8 +10,13 @@ def loadTalos(): robotComplete = example_robot_data.load("talos") qComplete = robotComplete.model.referenceConfigurations["half_sitting"] - locked_joints = [20,21,22,23,28,29,30,31] - locked_joints += [32, 33] + locked_joints_names = ["arm_left_5_joint", "arm_left_6_joint", "arm_left_7_joint", + "gripper_left_joint", + "arm_right_5_joint", "arm_right_6_joint", "arm_right_7_joint", + "gripper_right_joint", + "head_1_joint", + "head_2_joint"] + locked_joints = [robotComplete.model.getJointId(el) for el in locked_joints_names] robot = robotComplete.buildReducedRobot(locked_joints, qComplete) rmodel: pin.Model = robot.model q0 = rmodel.referenceConfigurations["half_sitting"] diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp index 6974eca..c4f2557 100644 --- a/include/simple-mpc/robot-handler.hpp +++ b/include/simple-mpc/robot-handler.hpp @@ -38,11 +38,6 @@ namespace simple_mpc */ double mass_; - /** - * @brief Joint id to be controlled in full model - */ - std::vector controlled_joints_ids_; - /** * @brief Reference configuration and velocity (most probably null velocity) * to use @@ -128,8 +123,6 @@ namespace simple_mpc return feet_names_; } - std::vector getControlledJointNames() const; - FrameIndex getBaseFrameId() const { return base_id_; diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp index a2677e9..b4477c7 100644 --- a/src/robot-handler.cpp +++ b/src/robot-handler.cpp @@ -1,6 +1,5 @@ #include "simple-mpc/robot-handler.hpp" -#include #include #include #include @@ -14,10 +13,6 @@ namespace simple_mpc const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name) : model_(model) { - // Controlled joints index - for (auto joint_name : model_.names) - controlled_joints_ids_.push_back(model.getJointId(joint_name)); - // Root frame id base_id_ = model_.getFrameId(base_frame_name); @@ -65,16 +60,6 @@ namespace simple_mpc return dx; } - std::vector RobotModelHandler::getControlledJointNames() const - { - std::vector joint_names; - for (JointIndex id : controlled_joints_ids_) - { - joint_names.push_back(model_.names.at(id)); - } - return joint_names; - } - RobotDataHandler::RobotDataHandler(const RobotModelHandler & model_handler) : model_handler_(model_handler) , data_(model_handler.getModel())