Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: accept frame transform from non-base frame #145

Closed
wants to merge 4 commits into from
Closed
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Next Next commit
feat: accept frame transform from non-base frame
JasperTan97 committed Oct 17, 2023
commit dd3bd8d4ab30f5bd10e510c341d9c28e80189fe5
6 changes: 4 additions & 2 deletions source/robot_model/include/robot_model/Model.hpp
Original file line number Diff line number Diff line change
@@ -125,7 +125,8 @@ class Model {
* @return the desired poses
*/
std::vector<state_representation::CartesianPose> forward_kinematics(const state_representation::JointPositions& joint_positions,
const std::vector<unsigned int>& frame_ids);
const std::vector<unsigned int>& frame_ids,
const unsigned int ref_frame_id = 0);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm temped to say that the reference frame has to be a vector too @eeberhard what do you think?
Or should we provide a vector of pairs?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you mean like multiple reference frames so we get back multiple viewpoints of the same frame, or rather we will get back num_of_frame_ids * num_of_ref_frame_ids back

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

More in the sense that if you provide a vector of frame ids, you would also need to provide a reference frame id for each frame id that you request.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IMO a common reference frame is sufficient. From an API side, it's simpler this way. I don't know how common it would be to request each frame in a separate reference frame, and even then you can just use state representation after the fact to transform a frame secondarily if it's super necessary (i.e., get elbow and wrist in base frame, then find wrist in elbow frame afterwards)


/**
* @brief Compute the forward kinematics, i.e. the pose of certain frames from the joint positions for a single frame
@@ -134,7 +135,8 @@ class Model {
* @return the desired pose
*/
state_representation::CartesianPose forward_kinematics(const state_representation::JointPositions& joint_positions,
unsigned int frame_id);
const unsigned int frame_id,
const unsigned int ref_frame_id = 0);

/**
* @brief Check if the vector's elements are inside the parameter limits
28 changes: 25 additions & 3 deletions source/robot_model/src/Model.cpp
Original file line number Diff line number Diff line change
@@ -221,17 +221,36 @@ Model::compute_gravity_torques(const state_representation::JointPositions& joint
}

state_representation::CartesianPose Model::forward_kinematics(const state_representation::JointPositions& joint_positions,
unsigned int frame_id) {
return this->forward_kinematics(joint_positions, std::vector<unsigned int>{frame_id}).front();
const unsigned int frame_id,
const unsigned int ref_frame_id) {
return this->forward_kinematics(joint_positions, std::vector<unsigned int>{frame_id}, ref_frame_id).front();
}

std::vector<state_representation::CartesianPose> Model::forward_kinematics(const state_representation::JointPositions& joint_positions,
const std::vector<unsigned int>& frame_ids) {
const std::vector<unsigned int>& frame_ids,
const unsigned int ref_frame_id) {
if (joint_positions.get_size() != this->get_number_of_joints()) {
throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()));
}
std::vector<state_representation::CartesianPose> pose_vector;
pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, joint_positions.data());

if (ref_frame_id >= static_cast<unsigned int>(this->robot_model_.nframes)) {
throw (exceptions::FrameNotFoundException(std::to_string(ref_frame_id)));
}
state_representation::CartesianPose ref_frame_pose = state_representation::CartesianPose::Identity(
this->robot_model_.frames[ref_frame_id].name);
if (ref_frame_id > 0) {
pinocchio::updateFramePlacement(this->robot_model_, this->robot_data_, ref_frame_id);
pinocchio::SE3 ref_pose = this->robot_data_.oMf[ref_frame_id];
Eigen::Vector3d ref_translation = ref_pose.translation();
Eigen::Quaterniond ref_quaternion;
pinocchio::quaternion::assignQuaternion(ref_quaternion, ref_pose.rotation());
ref_frame_pose = state_representation::CartesianPose(this->robot_model_.frames[ref_frame_id].name,
ref_translation,
ref_quaternion,
this->get_base_frame());
}
for (unsigned int id : frame_ids) {
if (id >= static_cast<unsigned int>(this->robot_model_.nframes)) {
throw (exceptions::FrameNotFoundException(std::to_string(id)));
@@ -245,6 +264,9 @@ std::vector<state_representation::CartesianPose> Model::forward_kinematics(const
translation,
quaternion,
this->get_base_frame());
if (ref_frame_id > 0) {
frame_pose = ref_frame_pose.inverse() * frame_pose;
}
pose_vector.push_back(frame_pose);
}
return pose_vector;