-
Notifications
You must be signed in to change notification settings - Fork 43
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
KDL based Movement in Cartesian Space #167
Changes from 4 commits
be1d057
e73fe11
af39f13
3bb01fc
3cd05b5
e033e3d
6deedad
104198c
b0a10a6
3db86da
23d0d6f
5f6ace3
4aa7b26
ffdada4
f2cad31
772699d
201bd89
6e7095e
aa5cb15
630bd5d
506ec03
4213fcc
7f4a662
ae04d88
29338f8
367004a
40f6067
7258070
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,217 @@ | ||
#include "rclcpp/rclcpp.hpp" | ||
#include "cmath" | ||
#include <iostream> | ||
#include <fstream> | ||
#include <sstream> | ||
#include <string> | ||
#include "chrono" | ||
#include "kdl_parser/kdl_parser.hpp" | ||
#include "kdl/chainfksolverpos_recursive.hpp" // for forward kinematics | ||
#include "kdl/chainiksolverpos_lma.hpp" // for inverse kinematics | ||
#include "lbr_fri_msgs/msg/lbr_position_command.hpp" | ||
#include "lbr_fri_msgs/msg/lbr_state.hpp" | ||
#include "friClientIf.h" | ||
|
||
|
||
using namespace std::chrono_literals; // this namespace to enable we can use 500ms to represent 500 milliseconds in our code. | ||
using std::placeholders::_1; | ||
|
||
lbr_fri_msgs::msg::LBRState current_robot_state; // store current robot state, including measured joint positions | ||
mhubii marked this conversation as resolved.
Show resolved
Hide resolved
|
||
KDL::Frame initial_cartesian_position; // we want to move the end effector in sine curve in z-direction in Cartesian space, this is the initial position | ||
|
||
class JointPositionPublisher:public rclcpp::Node | ||
{ | ||
private: | ||
rclcpp::Publisher<lbr_fri_msgs::msg::LBRPositionCommand>::SharedPtr publisher_; | ||
mhubii marked this conversation as resolved.
Show resolved
Hide resolved
|
||
rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr subscriber_; | ||
|
||
KDL::Chain chain_; // robot kinematics chain exetracted from robot URDF file | ||
|
||
double amplitude_; // rad | ||
double frequency_; // Hz | ||
double sampling_time_; // sampling time for the state of robot and sending position command | ||
double phase_; // initial phase | ||
int count_; | ||
|
||
private: | ||
/** | ||
* @function: callback function for Robot State Subscriber | ||
*/ | ||
void topic_callback(const lbr_fri_msgs::msg::LBRState& msg) | ||
{ | ||
current_robot_state = msg; | ||
|
||
if(count_ < 200) // in first 2 seconds, to make 'initial_position_command' be 'measured_joint_position' | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. might be better to introduce |
||
{ | ||
std::cout << "count_" << count_ << std::endl; | ||
double joint_position[7]; | ||
for(int i = 0; i < 7; i++) | ||
{ | ||
joint_position[i] = current_robot_state.measured_joint_position[i]; | ||
} | ||
initial_cartesian_position = computeForwardKinematics(chain_, joint_position); | ||
} | ||
else | ||
{ | ||
if (current_robot_state.session_state == KUKA::FRI::COMMANDING_ACTIVE) | ||
{ | ||
KDL::Frame cartesian_position_command = initial_cartesian_position; | ||
|
||
cartesian_position_command.p = KDL::Vector(initial_cartesian_position.p.x(), initial_cartesian_position.p.y(), initial_cartesian_position.p.z() + amplitude_ * sin(phase_)); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. oh okay I see, this performs oscillation in Cartesian space, could you maybe add a Cartesian pose topic or even better a Twist topic (to indicate velocity commands) instead? Twist commands might need some code changes |
||
|
||
phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_; | ||
|
||
unsigned int joint_number = chain_.getNrOfJoints(); // for kuka iiwa7 robot, this will be 7 | ||
KDL::JntArray current_joint_positions(joint_number); | ||
|
||
for(unsigned int i = 0; i < joint_number; i++) | ||
{ | ||
current_joint_positions(i) = current_robot_state.measured_joint_position[i]; | ||
} | ||
|
||
lbr_fri_msgs::msg::LBRPositionCommand joint_position_command = computeInverseKinematics(chain_, cartesian_position_command, current_joint_positions); | ||
|
||
publisher_->publish(joint_position_command); | ||
} | ||
} | ||
|
||
count_++; | ||
|
||
return; | ||
} | ||
|
||
public: | ||
JointPositionPublisher():Node("joint_position_publisher_node"), amplitude_(0.05), frequency_(0.5), sampling_time_(0.01) | ||
{ | ||
phase_ = 0.0; | ||
count_= 0.0; | ||
|
||
std::string urdf_file_path = "/home/nearlab-iiwa/lbr-stack/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa7/my_iiwa7.urdf"; // path of your robot urdf file | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. would it be possible to load this from the |
||
std::string robot_description_string = readUrdfFile(urdf_file_path); | ||
|
||
KDL::Tree robot_tree; | ||
if(!kdl_parser::treeFromString(robot_description_string, robot_tree)) | ||
{ | ||
std::cout << "Failed to construct kdl tree." << std::endl; | ||
} | ||
|
||
std::string root_link = "link_0"; // adjust with your URDF‘s root link | ||
std::string tip_link = "link_ee"; // adjust with your URDF‘s tip link | ||
if(!robot_tree.getChain(root_link, tip_link, chain_)) | ||
{ | ||
std::cerr << "Failed to get chain from tree." << std::endl; | ||
} | ||
else | ||
{ | ||
std::cout << "Get chain from tree successfully." << std::endl; | ||
} | ||
|
||
publisher_ = this->create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command/joint_position", 10); | ||
subscriber_ = this->create_subscription<lbr_fri_msgs::msg::LBRState>("/lbr/state", 10, std::bind(&JointPositionPublisher::topic_callback, this, _1)); | ||
} | ||
|
||
/** | ||
* @function: convert URDF file to a string | ||
* @param urdf_file_path the path of URDF file | ||
* @return string type of URDF file | ||
*/ | ||
std::string readUrdfFile(const std::string& urdf_file_path) | ||
{ | ||
std::ifstream file_stream(urdf_file_path); | ||
if(!file_stream) // if open this file failed, return null string | ||
{ | ||
std::cerr << "Failed to open file at path:" << urdf_file_path << std::endl; | ||
return ""; | ||
} | ||
|
||
std::stringstream buffer; | ||
buffer << file_stream.rdbuf(); | ||
return buffer.str(); | ||
} | ||
|
||
/** | ||
* @function: calculate forward kinematics of robot | ||
* @param chain robot kinematics chain parsed from URDF file | ||
* @param position_array_ptr seven joint positions of robot store in double position_array_ptr[7] | ||
* @return no value | ||
*/ | ||
KDL::Frame computeForwardKinematics(KDL::Chain& chain, double* position_array_ptr) | ||
{ | ||
KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain); | ||
|
||
unsigned int joint_number = chain.getNrOfJoints(); | ||
KDL::JntArray joint_positions = KDL::JntArray(joint_number); | ||
|
||
for(unsigned int i = 0; i < joint_number; i++) | ||
{ | ||
joint_positions(i) = position_array_ptr[i]; | ||
} | ||
|
||
KDL::Frame cartesian_position; | ||
|
||
if(fk_solver.JntToCart(joint_positions, cartesian_position) < 0) | ||
{ | ||
std::cerr << "FK Solver to calculate JointToCartesian failed." << std::endl; | ||
} | ||
else | ||
{ | ||
double x, y, z, roll, pitch, yaw; | ||
x = cartesian_position.p.x(); // get position | ||
y = cartesian_position.p.y(); | ||
z = cartesian_position.p.z(); | ||
|
||
cartesian_position.M.GetRPY(roll, pitch, yaw); // get RPY value | ||
|
||
//std::cout << "Position: [" << x << ", " << y << ", " << z << "]" << std::endl; | ||
//std::cout << "Orientation (RPY): [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl; | ||
} | ||
|
||
return cartesian_position; | ||
} | ||
|
||
/** | ||
* @function: calculate inverse kinematics of robot | ||
* @param chain kinematics chain of robot got from URDF file | ||
* @param desired_end_effector_pose target cartesian pose we want to transform to joint space | ||
* @param current_joint_positions current joint positions | ||
* @return joint positions command | ||
*/ | ||
lbr_fri_msgs::msg::LBRPositionCommand computeInverseKinematics(KDL::Chain& chain, KDL::Frame& desired_end_effector_pose, KDL::JntArray& current_joint_positions) | ||
{ | ||
KDL::ChainIkSolverPos_LMA ik_solver(chain); | ||
KDL::JntArray result_joint_positions = KDL::JntArray(chain.getNrOfJoints()); | ||
lbr_fri_msgs::msg::LBRPositionCommand joint_position_command; | ||
|
||
auto start = std::chrono::high_resolution_clock::now(); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. maybe only measure time on debug |
||
int ik_result = ik_solver.CartToJnt(current_joint_positions, desired_end_effector_pose, result_joint_positions); // if solving failed, return value would be less than 0 | ||
auto end = std::chrono::high_resolution_clock::now(); | ||
std::chrono::duration<double, std::milli> execution_time = end - start; | ||
std::cout << "IK solver execution time: " << execution_time.count() << "ms" << std::endl; | ||
|
||
if (ik_result < 0) | ||
{ | ||
std::cerr << "Inverse kinematics failed." << std::endl; | ||
} | ||
else | ||
{ | ||
//std::cout << "Inverse kinematics solution:" << std::endl; | ||
for (unsigned int i = 0; i < result_joint_positions.data.size(); i++) | ||
{ | ||
joint_position_command.joint_position[i] = result_joint_positions(i); | ||
//std::cout << "Joint " << i << ": " << result_joint_positions(i) << std::endl; | ||
} | ||
} | ||
|
||
return joint_position_command; | ||
} | ||
}; | ||
|
||
int main(int argc, char** argv) | ||
{ | ||
rclcpp::init(argc, argv); | ||
|
||
rclcpp::spin(std::make_shared<JointPositionPublisher>()); | ||
|
||
rclcpp::shutdown(); | ||
return 0; | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
this dependency should be replaced by
orocos_kdl_vendor
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
okay, I get it, thank you.