Skip to content

Commit

Permalink
[robots] Supported fixed-based robots
Browse files Browse the repository at this point in the history
  • Loading branch information
@cmastalli
cmastalli committed Feb 22, 2024
1 parent ee1e67a commit 136d37d
Showing 1 changed file with 25 additions and 20 deletions.
45 changes: 25 additions & 20 deletions include/crocoddyl_msgs/conversions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,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 nv_root = model.joints[root_joint_id].nv();
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 njoints = model.nv - nv_root;
if (tau.size() != static_cast<int>(njoints) && tau.size() != 0) {
throw std::invalid_argument("Expected tau to be 0 or " +
Expand Down Expand Up @@ -236,7 +237,7 @@ static inline void toMsg(
msg.centroidal.base_angular_velocity.x = v(3);
msg.centroidal.base_angular_velocity.y = v(4);
msg.centroidal.base_angular_velocity.z = v(5);
} else if (nv_root != 0) {
} else if (nv_root > 0) {
std::cerr
<< "Warning: toMsg conversion does not yet support root joints "
"different to a floating base. We cannot publish base information."
Expand All @@ -262,10 +263,10 @@ static inline void toMsg(
// Filling the joint state
msg.joints.resize(njoints);
for (std::size_t j = 0; j < njoints; ++j) {
msg.joints[j].name = model.names[2 + j];
msg.joints[j].position = q(model.joints[1].nq() + j);
msg.joints[j].velocity = v(model.joints[1].nv() + j);
msg.joints[j].acceleration = a(model.joints[1].nv() + j);
msg.joints[j].name = model.names[root_joint_id + j + 1];
msg.joints[j].position = q(nq_root + j);
msg.joints[j].velocity = v(nv_root + j);
msg.joints[j].acceleration = a(nv_root + j);
if (tau.size() != 0) {
msg.joints[j].effort = tau(j);
}
Expand Down Expand Up @@ -537,7 +538,7 @@ fromMsg(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
" but received " + std::to_string(v.size()));
}
const std::size_t root_joint_id = getRootJointId(model);
const std::size_t nv_root = model.joints[root_joint_id].nv();
const std::size_t nv_root = model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0;
const std::size_t njoints = model.nv - nv_root;
if (tau.size() != static_cast<int>(njoints)) {
throw std::invalid_argument("Expected tau to be " +
Expand Down Expand Up @@ -587,7 +588,7 @@ fromMsg(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
.toRotationMatrix()
.transpose() *
v.head<3>(); // local frame
} else if (nv_root != 0) {
} else if (nv_root > 0) {
std::cerr
<< "Warning: fromMsg conversion does not yet support root joints "
"different to a floating base. We cannot publish base information."
Expand Down Expand Up @@ -676,8 +677,10 @@ static inline void fromReduced(
const Eigen::Ref<const Eigen::VectorXd> &qref,
const std::vector<pinocchio::JointIndex> &locked_joint_ids) {
const std::size_t root_joint_id = getRootJointId(model);
const std::size_t nq_root = model.joints[root_joint_id].nq();
const std::size_t nv_root = model.joints[root_joint_id].nv();
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 njoints = model.nv - nv_root;
const std::size_t njoints_reduced = reduced_model.nv - nv_root;
if (q_out.size() != model.nq) {
throw std::invalid_argument("Expected q_out to be " +
std::to_string(model.nq) + " but received " +
Expand All @@ -698,14 +701,14 @@ static inline void fromReduced(
std::to_string(reduced_model.nv) +
" but received " + std::to_string(v_in.size()));
}
if (static_cast<std::size_t>(tau_out.size()) != model.nv - nv_root) {
if (static_cast<std::size_t>(tau_out.size()) != njoints) {
throw std::invalid_argument(
"Expected tau_out to be " + std::to_string(model.nv - nv_root) +
"Expected tau_out to be " + std::to_string(njoints) +
" but received " + std::to_string(tau_out.size()));
}
if (static_cast<std::size_t>(tau_in.size()) != reduced_model.nv - nv_root) {
if (static_cast<std::size_t>(tau_in.size()) != njoints_reduced) {
throw std::invalid_argument(
"Expected tau_in to be " + std::to_string(reduced_model.nv - nv_root) +
"Expected tau_in to be " + std::to_string(njoints_reduced) +
" but received " + std::to_string(tau_in.size()));
}

Expand Down Expand Up @@ -755,8 +758,10 @@ toReduced(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
const Eigen::Ref<const Eigen::VectorXd> &v_in,
const Eigen::Ref<const Eigen::VectorXd> &tau_in) {
const std::size_t root_joint_id = getRootJointId(model);
const std::size_t nq_root = model.joints[root_joint_id].nq();
const std::size_t nv_root = model.joints[root_joint_id].nv();
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 njoints = model.nv - nv_root;
const std::size_t njoints_reduced = reduced_model.nv - nv_root;
if (q_out.size() != reduced_model.nq) {
throw std::invalid_argument(
"Expected q_out to be " + std::to_string(reduced_model.nq) +
Expand All @@ -777,14 +782,14 @@ toReduced(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
std::to_string(model.nv) + " but received " +
std::to_string(v_in.size()));
}
if (static_cast<std::size_t>(tau_out.size()) != reduced_model.nv - nv_root) {
if (static_cast<std::size_t>(tau_out.size()) != njoints_reduced) {
throw std::invalid_argument(
"Expected tau_out to be " + std::to_string(reduced_model.nv - nv_root) +
"Expected tau_out to be " + std::to_string(njoints_reduced) +
" but received " + std::to_string(tau_out.size()));
}
if (static_cast<std::size_t>(tau_in.size()) != model.nv - nv_root) {
if (static_cast<std::size_t>(tau_in.size()) != njoints) {
throw std::invalid_argument(
"Expected tau_in to be " + std::to_string(model.nv - nv_root) +
"Expected tau_in to be " + std::to_string(njoints) +
" but received " + std::to_string(tau_in.size()));
}

Expand Down

0 comments on commit 136d37d

Please sign in to comment.