-
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
Merged
mhubii
merged 28 commits into
lbr-stack:dev-humble-cart-vel-demo
from
BoWangFromMars:BoWangFromMars-patch-1
Apr 22, 2024
Merged
Changes from 18 commits
Commits
Show all changes
28 commits
Select commit
Hold shift + click to select a range
be1d057
KDL tutorial
BoWangFromMars e73fe11
Cartesian space movement
BoWangFromMars af39f13
Update CMakeLists.txt
BoWangFromMars 3bb01fc
Update package.xml
BoWangFromMars 3cd05b5
Update CMakeLists.txt
BoWangFromMars e033e3d
Create cartesian_pose_node.cpp
BoWangFromMars 6deedad
Create iiwa7.urdf
BoWangFromMars 104198c
Create iiwa14.urdf
BoWangFromMars b0a10a6
Create med7.urdf
BoWangFromMars 3db86da
Create med14.urdf
BoWangFromMars 23d0d6f
Delete lbr_demos/lbr_demos_fri_ros2_cpp/doc/KDL installation and demo…
BoWangFromMars 5f6ace3
Add files via upload
BoWangFromMars 4aa7b26
Update cartesian_pose_node.cpp
BoWangFromMars ffdada4
Update cartesian_pose_node.cpp
BoWangFromMars f2cad31
Update cartesian_path_planning_node.cpp
BoWangFromMars 772699d
Update cartesian_path_planning_node.cpp
BoWangFromMars 201bd89
Update cartesian_pose_node.cpp
BoWangFromMars 6e7095e
Update package.xml
BoWangFromMars aa5cb15
Delete lbr_description/urdf/iiwa14/iiwa14.urdf
BoWangFromMars 630bd5d
Delete lbr_description/urdf/iiwa7/iiwa7.urdf
BoWangFromMars 506ec03
Delete lbr_description/urdf/med14/med14.urdf
BoWangFromMars 4213fcc
Delete lbr_description/urdf/med7/med7.urdf
BoWangFromMars 7f4a662
Delete lbr_demos/lbr_demos_fri_ros2_cpp/doc/KDL installation.pdf
BoWangFromMars ae04d88
Update package.xml
BoWangFromMars 29338f8
Update CMakeLists.txt
BoWangFromMars 367004a
Update CMakeLists.txt
BoWangFromMars 40f6067
Update cartesian_pose_node.cpp
BoWangFromMars 7258070
Add files via upload
BoWangFromMars File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Binary file not shown.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -12,6 +12,12 @@ | |
<depend>fri_vendor</depend> | ||
<depend>lbr_fri_msgs</depend> | ||
<depend>rclcpp</depend> | ||
<depend>tf2_ros</depend> | ||
<depend>urdf</depend> | ||
<depend>geometry_msgs</depend> | ||
<depend>sensor_msgs</depend> | ||
<depend>orocos_kdl</depend> | ||
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. this dependency should be replaced by 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. okay, I get it, thank you. |
||
<depend>kdl_parser</depend> | ||
|
||
<exec_depend>lbr_fri_ros2</exec_depend> | ||
|
||
|
@@ -21,4 +27,4 @@ | |
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> | ||
</package> |
74 changes: 74 additions & 0 deletions
74
lbr_demos/lbr_demos_fri_ros2_cpp/src/cartesian_path_planning_node.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,74 @@ | ||
#include "rclcpp/rclcpp.hpp" | ||
#include "cmath" | ||
#include <iostream> | ||
#include <string> | ||
#include "geometry_msgs/msg/pose.hpp" | ||
|
||
using std::placeholders::_1; | ||
|
||
class CartesianPosePublisherNode:public rclcpp::Node | ||
{ | ||
private: | ||
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_publisher_; | ||
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_subscriber_; | ||
|
||
geometry_msgs::msg::Pose initial_cartesian_pose_; // robot starts from this pose | ||
bool is_init_; | ||
|
||
double amplitude_; // rad | ||
double frequency_; // Hz | ||
double sampling_time_; // sampling time for sending position command | ||
double phase_; // initial phase | ||
|
||
|
||
private: | ||
/** | ||
* @function: callback function for Cartesian Pose Subscriber | ||
* @param msg Cartesian Pose of the robot | ||
*/ | ||
void topic_callback(const geometry_msgs::msg::Pose& msg) | ||
{ | ||
if(!is_init_) | ||
{ | ||
initial_cartesian_pose_ = msg; | ||
is_init_ = true; | ||
} | ||
else | ||
{ | ||
geometry_msgs::msg::Pose cartesian_pose_command = initial_cartesian_pose_; | ||
|
||
phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_; | ||
cartesian_pose_command.position.z += amplitude_ * sin(phase_); | ||
|
||
cartesian_pose_publisher_->publish(cartesian_pose_command); | ||
} | ||
|
||
return; | ||
} | ||
|
||
public: | ||
CartesianPosePublisherNode():Node("cartesian_pose_publisher_node") | ||
{ | ||
is_init_ = false; | ||
amplitude_ = 0.05; | ||
frequency_ = 0.5; | ||
sampling_time_ = 0.01; | ||
phase_ = 0.0; | ||
|
||
cartesian_pose_publisher_ = this->create_publisher<geometry_msgs::msg::Pose>( | ||
"/lbr/command/cartesian_pose", 10); | ||
cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>( | ||
"/lbr/state/cartesian_pose", 10, | ||
std::bind(&CartesianPosePublisherNode::topic_callback, this, _1)); | ||
} | ||
}; | ||
|
||
int main(int argc, char** argv) | ||
{ | ||
rclcpp::init(argc, argv); | ||
|
||
rclcpp::spin(std::make_shared<CartesianPosePublisherNode>()); | ||
|
||
rclcpp::shutdown(); | ||
return 0; | ||
} |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
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.
one more question regarding this doc. KDL is usually installed through
rosdep
as listed inpackage.xml
files, refer e.g.lbr_fri_ros2_stack/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml
Line 18 in 036b23b