Skip to content

Finish the method for forward kinematics #4

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.

Sign up for GitHub

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

Merged
merged 4 commits into from
May 7, 2021
Merged

Finish the method for forward kinematics #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b4957ca
Rename incremental_ik_calculator -> incremental_kinematics
May 7, 2021
0e5f9dc
Use Eigen double types. Rename _trafo transform variables
May 7, 2021
07da5c4
Fix typo in transformation matrix
May 7, 2021
1be6233
Implement incremental forward kinematics
May 7, 2021
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
2 changes: 1 addition & 1 deletion admittance_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ add_library(
SHARED
src/admittance_controller.cpp
src/admittance_rule.cpp
src/incremental_ik_calculator.cpp
src/incremental_kinematics.cpp
)
target_include_directories(
admittance_controller
Expand Down
4 changes: 2 additions & 2 deletions admittance_controller/include/admittance_controller/admittance_rule.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_
#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_

#include "admittance_controller/incremental_ik_calculator.hpp"
#include "admittance_controller/incremental_kinematics.hpp"
#include "control_msgs/msg/admittance_controller_state.hpp"
#include "controller_interface/controller_interface.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
Expand Down Expand Up @@ -86,7 +86,7 @@ class AdmittanceRule

protected:
// IK variables
std::shared_ptr<IncrementalIKCalculator> ik_;
std::shared_ptr<IncrementalKinematics> ik_;
Eigen::VectorXd delta_x_;
Eigen::VectorXd delta_theta_;

Expand Down
12 changes: 6 additions & 6 deletions ..._controller/incremental_ik_calculator.hpp → ...nce_controller/incremental_kinematics.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,34 +25,34 @@
namespace admittance_controller
{

class IncrementalIKCalculator
class IncrementalKinematics
{
public:
/**
* \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta.
* It uses the Jacobian from MoveIt.
*/
IncrementalIKCalculator(const std::shared_ptr<rclcpp::Node> & node, const std::string & group_name);
IncrementalKinematics(const std::shared_ptr<rclcpp::Node> & node, const std::string & group_name);

/**
* \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian.
* \param delta_x_vec input Cartesian deltas (x, y, z, rx, ry, rz)
* \param ik_base_to_tip_trafo transformation between ik base and ik tip
* \param ik_base_to_tip_tf transformation between ik base and ik tip
* \param delta_theta_vec output vector with joint states
* \return true if successful
*/
bool
convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_theta_vec);
convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & delta_theta_vec);

/**
* \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian.
* \param[in] delta_theta_vec vector with joint states
* \param[in] ik_base_to_tip_trafo transformation between ik base to ik tip
* \param[in] ik_base_to_tip_tf transformation between ik base to ik tip
* \param[out] delta_x_vec Cartesian deltas (x, y, z, rx, ry, rz)
* \return true if successful
*/
bool
convertJointDeltasToCartesianDeltas(const std::vector<double> & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_x_vec);
convertJointDeltasToCartesianDeltas(const std::vector<double> & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & delta_x_vec);

private:
// MoveIt setup, required to retrieve the Jacobian
Expand Down
2 changes: 1 addition & 1 deletion admittance_controller/src/admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
// #include "Eigen/Core"

#include "admittance_controller/admittance_controller.hpp"
#include "admittance_controller/incremental_ik_calculator.hpp"
#include "admittance_controller/incremental_kinematics.hpp"
#include "controller_interface/helpers.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"

Expand Down
2 changes: 1 addition & 1 deletion admittance_controller/src/admittance_rule.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared
relative_desired_joint_state_vec_.reserve(6);

// Initialize IK
ik_ = std::make_shared<IncrementalIKCalculator>(node, ik_group_name_);
ik_ = std::make_shared<IncrementalKinematics>(node, ik_group_name_);

return controller_interface::return_type::OK;
}
Expand Down
64 changes: 54 additions & 10 deletions ...troller/src/incremental_ik_calculator.cpp → ...controller/src/incremental_kinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,14 +14,14 @@
//
/// \author: Andy Zelenak

#include "admittance_controller/incremental_ik_calculator.hpp"
#include "admittance_controller/incremental_kinematics.hpp"

#include "Eigen/VectorXf"
#include "Eigen/VectorXd"
#include "tf2_eigen/tf2_eigen.h"

namespace admittance_controller
{
IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr<rclcpp::Node> & node, const std::string & group_name) : node_(node)
IncrementalKinematics::IncrementalKinematics(const std::shared_ptr<rclcpp::Node> & node, const std::string & group_name) : node_(node)
{
// TODO(andyz): Parameterize robot description and joint group
std::unique_ptr<robot_model_loader::RobotModelLoader> model_loader_ptr =
Expand All @@ -34,23 +34,25 @@ IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr<rclcpp::N
// By default, the MoveIt Jacobian frame is the last link
}

bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_theta_vec)
bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & delta_theta_vec)
{
// see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector
Eigen::VectorXf delta_x = Eigen::Map<Eigen::VectorXf, Eigen::Unaligned>(&delta_x_vec[0], delta_x_vec.size());
Eigen::VectorXd delta_x = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&delta_x_vec[0], delta_x_vec.size());

// Transform delta_x to the moveit_jacobian_frame
// TODO: replace when this PR to tf2_eigen is merged
// https://github.com/ros2/geometry2/pull/406
try
{
// 4x4 transformation matrix
const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_trafo);
const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf);

// Build the 6x6 transformation matrix
Eigen::MatrixXd twist_transform(6,6);
// upper left 3x3 block is the rotation part
twist_transform.block(0,0,3,3) = affine_transform.rotation();
// upper right 3x3 block is all zeros
twist_transform.block(0,4,3,3) = Eigen::MatrixXd::Zero(3,3);
twist_transform.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3);
// lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf
Eigen::MatrixXd pos_vector_3x3(3,3);
pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y();
Expand All @@ -73,17 +75,59 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vec
svd_ = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV);
matrix_s_ = svd_.singularValues().asDiagonal();
pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose();
Eigen::VectorXf delta_theta = pseudo_inverse_ * delta_x;
Eigen::VectorXd delta_theta = pseudo_inverse_ * delta_x;

std::vector<double> delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows());
delta_theta_vec = delta_theta_v;

return true;
}

bool IncrementalIKCalculator::convertJointDeltasToCartesianDeltas(const std::vector<double> & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_x_vec)
bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector<double> & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & delta_x_vec)
{
// TODO(andyz): Please add here FK implementation
// see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector
Eigen::VectorXd delta_theta = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&delta_theta_vec[0], delta_theta_vec.size());

// Multiply with the Jacobian to get delta_x
jacobian_ = kinematic_state_->getJacobian(joint_model_group_);
// delta_x will be in the working frame of MoveIt
Eigen::VectorXd delta_x = jacobian_ * delta_theta;

// Transform delta_x to the tip frame
// TODO: replace when this PR to tf2_eigen is merged
// https://github.com/ros2/geometry2/pull/406
try
{
// 4x4 transformation matrix
const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf);

// Build the 6x6 transformation matrix
Eigen::MatrixXd twist_transform(6,6);
// upper left 3x3 block is the rotation part
twist_transform.block(0,0,3,3) = affine_transform.rotation();
// upper right 3x3 block is all zeros
twist_transform.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3);
// lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf
Eigen::MatrixXd pos_vector_3x3(3,3);
pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y();
pos_vector_3x3(1, 0) = affine_transform.translation().z(); pos_vector_3x3(1,1) = 0; pos_vector_3x3(1,2) = -affine_transform.translation().x();
pos_vector_3x3(2, 0) = -affine_transform.translation().y(); pos_vector_3x3(2,1) = affine_transform.translation().x(); pos_vector_3x3(1,2) = 0;
twist_transform.block(3,0,3,3) = pos_vector_3x3 * affine_transform.rotation();
// lower right 3x3 block is the rotation part
twist_transform.block(3,3,3,3) = affine_transform.rotation();

delta_x = twist_transform * delta_x;
}
catch (tf2::TransformException & ex)
{
RCLCPP_ERROR(node_->get_logger(), "Transformation of wrench failed.");
return false;
}

std::vector<double> delta_x_v(&delta_x[0], delta_x.data() + delta_x.cols() * delta_x.rows());
delta_x_vec = delta_x_v;

return true;
}

} // namespace admittance_controller