RBE500: Foundation of Robotics Final Project
GPL-3.0 License
The aim of the assignment is meant to get a better understanding of basic concepts of Robotics using tools like ROS2 Humble Hawksbill, Gazebo Sim, RVIZ and MathWorks® MATLAB The final project is divided into three seperate assignments:
Assignment 1: Build Model URDF, Forward Kinematics Node and Inverse Kinematics Node.
Assignment 2: Build a node to control Joint States.
Assignment 3: Build a node to control Joint or End-Effector Velocity.
🗒 Note: This project is designed on Ubuntu 22.04 LTS and not tested on any other platforms
⚠ Warning: Remember, If anything doesnt work as it is supposed to, just use the rules of IT. Close it and restart it again 😃
Set locale
Make sure you have a locale which supports UTF-8
. If you are in a minimal environment (such as a docker container), the locale may be something minimal like POSIX
. We test with the following settings. However, it should be fine if you’re using a different UTF-8 supported locale.
locale # check for UTF-8
sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
locale # verify settings
Setup Sources
You will need to add the ROS 2 apt repository to your system.
First ensure that the Ubuntu Universe repository is enabled.
sudo apt install software-properties-common
sudo add-apt-repository universe
Now add the ROS 2 GPG key with apt.
sudo apt update && sudo apt install curl
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
Then add the repository to your sources list.
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
Install ROS 2 Packages
Update your apt repository caches after setting up the repositories.
sudo apt update
ROS 2 packages are built on frequently updated Ubuntu systems. It is always recommended that you ensure your system is up to date before installing new packages.
sudo apt upgrade
Desktop Install (Recommended): ROS, RViz, demos, tutorials.
sudo apt install ros-humble-desktop -y
Sourcing the setup script
Set up your environment by sourcing the following file.
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
Extra steps just to ensure colcon is properly installed
The ROS project hosts copies of the Debian packages in their apt repositories.
sudo sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
After that you can install the Debian package which depends on colcon-core
as well as commonly used extension packages (see setup.cfg).
sudo apt update
sudo apt install python3-colcon-common-extensions
In this tutorial we will install Gazebo 11 and all its supporting files that will aid us in robot simulation.
Gazebo
Use the following commands to install Gazebo and its supplementry files
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
Once this is done, update using and make sure it runs without any errors
sudo apt-get update
Now install Gazebo using
sudo apt-get install gazebo libgazebo-dev
You can check your installation by running this in a new terminal
gazebo
Gazebo ROS 2 packages
To use Gazebo with ROS2, there are certain packages that need to be installed
sudo apt install ros-humble-gazebo-ros-pkgs
Now we will install simulated robot controllers
sudo apt-get install ros-humble-ros2-control ros-humble-ros2-controllers
sudo apt-get install ros-humble-gazebo-ros2-control ros-humble-xacro
sudo apt-get install ros-humble-joint-state-publisher ros-humble-joint-state-publisher-gui
Now your system is ready to simulate any robot.
Spawn rrbot in Gazebo
Before doing colcon build
we have to install some dependencies for the package to function correctly.
Run the following commands in terminal
sudo apt install python3-pip
sudo pip3 install transforms3d
These are optional steps just to make sure everything is alright:
Please find the zipped package that has one of many ways to spawn an URDF in Gazebo using ROS2. The unedited files can be found in the directory:
/Resources/Group Project 1/rrbot_simulation_files.zip
Extract the package to your /src
.
Once you have successfully installed all the required packages, find the supporting zip file that contains rrbot simulation files.
RRBOT:
That zip file contains 2 packages:
The launch folder contains a launch file rrbot_world.launch.py which will be used to launch every thing, gazebo simulation environment, controller manager for ros2 control, robot state publisher.
In the main folder run the following commands:
colcon build --symlink-install
. install/setup.bash
As we are using some gazebo plugins for camera, we need to source the gazebo setup bash file too
echo ". /usr/share/gazebo/setup.bash" >> ~/.bashrc
Now open a new terminal to launch rrbot in gazebo and run
ros2 launch rrbot_gazebo rrbot_world.launch.py
This should result in a new Gazebo window popping up with rrbot spawned, Now if you do ros2 topic list, you will be able to see a bunch of different topics /joint_states : This topic will have the robot's current state, i.e. position, velocity and efforts of each joint /forward_position_controller/commands : This is the control topic, this will be used to control the robot joints /camera1/image_raw : On this topic the robot will publish the camera feed, to visualize what the camera is seeing we can use rviz2.
🗒 Note: This can also result in your system getting hanged for a few minutes, this is just a first time thing and please don't kill the node if this happens, average hang-time is around 2-3 mins, if it goes above 10 mins kill the launch file.
Plot Juggler is a tool used for visualizing the topics with data for plotting.
Installation of Plot Juggler is pretty simple. Since we are using ROS2 Humble, we will install for the same edition of ROS.
Install the Plot Juggler using the command:
sudo apt-get install ros-humble-plotjuggler-ros
And we are all done with Plot Juggler Installation.
Compile the project by using the command:
colcon build
Launch the RVIZ model in a new terminal using:
ros2 launch rrbot_description view_robot.launch.py
This will launch a Rviz windows which shows the robot model along with a a small window titled Joint State Publisher with three sliders to control three joints.
In a new Terminal, Launch the Forward Kinematics node using:
ros2 run rrbot_gazebo fkin_publisher
The Node will take the Joint States from the topic /joint_states
in the format sensor_msgs::msg::JointState
The Calculated Pose is published on the topic /forward_position_controller/commands
in format std_msgs::msg::Float64MultiArray
In a new Terminal, Echo the Forward Kinematics Topic using
ros2 topic echo /forward_position_controller/commands
The output data will be displayed as follows:
The x coordinates is displayed at line 4.
The y coordinates is displayed at line 8.
The z coordinates is displayed at line 12.
The Inverse Kinematics Node works on a server service node setup. The user can type the end-effector pose after launching the server node and request node to calculate the Joint States using a Service Call.
In a new terminal (You can close all other terminals if you wish to), Launch the Inverse Kinematics Server Node using:
ros2 run rrbot_gazebo ikin_publisher
In a new terminal launch the service using
ros2 service call /inverse_kinematics custom_interfaces/srv/FindJointStates "{x: 1, y: 0, z: 2}"
Change the end effector position by changing the values for (x,y,z).
The service Call will return the output in following format:
The service results two sets of possible response:
Compile the project by using the command:
colcon build
Launch RRBOT in Gazebo using:
ros2 launch rrbot_gazebo rrbot_world.launch.py
This will launch the Gazebo Window with the RRBOT spawned.
Launch the Joint Control Node using:
ros2 run rrbot_gazebo joint_effort_control
The control strategy used for the Joint Control Strategy is PD Control. The system is broken down into individual joints where each joint has it's own control system. This strategy divides the whole system into 3 individual SISO System allowing us to tune Kp and Kd parameters individually.
$$u = -K_{p}*e - K_{d}\dot{e}$$
where:
Call the Joint Control service using:
ros2 service call /joint_state_controller custom_interfaces/srv/SetJointStates '{rq1: 1, rq2: 2, rq3: 2}'
Change the Joint States by trying different values for (rq1,rq2,rq3)
Right Click the Video below and open in new tab to see an example
Echo the topic to see the reference Joint Position:
ros2 topic echo /reference_joint_states/commands
We can visualize the efforts and joint states using PlotJuggler.
Launch Plot Juggler using the following:
ros2 run plotjuggler plotjuggler
Once the plot Juggler is launched, configure the system to view the joint data as follows:
ROS2 Topic Subscriber
and Buffer to 60
and press Start Button as shown:Select ROS messages
Select the Following three topics from list:
/forward_effort_controller/commands
/joint_states
/reference_joint_states/commands
Right Click on the tab1 canvas
and Select Split Horizontally
twice so you get a layout as shown below:
Drag and Drop the Following Topics in each Canvas from the Publishers Tab:
/forward_effort_controller/commands/data.0
/joint_states/joint1/position
/joint_states/joint1/velocity
/reference_joint_states/commands/data.0
/forward_effort_controller/commands/data.1
/joint_states/joint2/position
/joint_states/joint2/velocity
/reference_joint_states/commands/data.1
/forward_effort_controller/commands/data.2
/joint_states/joint3/position
/joint_states/joint3/velocity
/reference_joint_states/commands/data.2
The complete setup for visualization should look as below:
Compile the project by using the command:
colcon build
Launch RRBOT in Gazebo using:
ros2 launch rrbot_gazebo rrbot_world.launch.py
This will launch the Gazebo Window with the RRBOT spawned.
Launch the Velocity Control Node using:
ros2 run rrbot_gazebo end_eff_vel_control
The control strategy used for the Velocity Control Strategy is PD Control. The system is broken down into individual joints where each joint has it's own control system. This strategy divides the whole system into 3 individual SISO System allowing us to tune Kp and Kd parameters individually.
$$u = -K_{p}*e - K_{d}\dot{e}$$
where:
Velocity Control can be done in two way:
ros2 service call /joint_velocity_service custom_interfaces/srv/SetJointVelocity "{vq1: 1, vq2: 0, vq3: 0}"
ros2 service call /end_effector_velocity_service custom_interfaces/srv/SetEndEffectorVelocity "{vx: 1, vy: 0, vz: 0}"
Right Click the Video below and open in new tab to see an example
We can visualize the efforts and joint states using PlotJuggler. Launch Plot Juggler using the following:
ros2 run plotjuggler plotjuggler
Once the plot Juggler is launched, configure the system to view the joint data as follows:
ROS2 Topic Subscriber
and Buffer to 60
and press Start Button as shown:Select ROS messages
Select the Following three topics from list:
/forward_effort_controller/commands
/joint_states
/reference_velocities/end_effector
/reference_velocities/joints
Right Click on the tab1 canvas
and Select Split Horizontally
twice so you get a layout as shown below:
Drag and Drop the Following Topics in each Canvas from the Publishers Tab:
/forward_effort_controller/commands/data.0
/joint_states/joint1/velocity
/reference_velocities/end_effector/data.0
/reference_velocities/joints/data.0
/forward_effort_controller/commands/data.1
/joint_states/joint2/velocity
/reference_velocities/end_effector/data.1
/reference_velocities/joints/data.1
/forward_effort_controller/commands/data.2
/joint_states/joint3/velocity
/reference_velocities/end_effector/data.2
/reference_velocities/joints/data.2
The complete setup for visualization should look as below:
This node is the Forward Kinematics Node. It takes Joint Position as input and calculates End Effector Pose using it.
Include Essential File Headers
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <math.h> /* round, floor, ceil, trunc */
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
Generating a Forward Kinematics Publisher Node
rclcpp::spin(std::make_shared<FKin_Publisher>());
Define Robot Physical Parameters
std::double_t l1 = 1, l2 = 1, ao = 0.05, lb = 1;
Create a subscriber that recieves Joint States from /joint_states
and publishes the Robot Pose to /forward_position_controller/commands
.
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr fkin_publisher_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
We need a publisher and subscriber for our operation.
fkin_publisher
: This publisher is used to publish the calculated pose from the recieved joint states.joint_state_subscriber
: The subscriber subscribes to the /joint_states
topic and the is binded to the callback function void topic_callback(...) const
using the std::bind(&FKin_Publisher::topic_callback, this, _1)
command.FKin_Publisher() : Node("minimal_publisher"), count_(0)
{
fkin_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("/forward_position_controller/commands", 10);
joint_state_subscriber_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10, std::bind(&FKin_Publisher::topic_callback, this, _1));
}
The callback function is called everytime the Joint State subscriber recieves new Joint Values.
void topic_callback(const sensor_msgs::msg::JointState::SharedPtr msg) const
The Joint Position is stored in a Vector array
std::vector<std::double_t> joint_states = {msg->position[0],msg->position[1],msg->position[2]};
std::cout << joint_states[0] << "," << joint_states[1] << "," << joint_states[2] << std::endl;
Pose $(T)$ is calculated using the joint states
using the formula
T=
\begin{bmatrix}
cos(\theta_{1}+\theta_{2})&-sin(\theta_{1}+\theta_{2})&0&cos(\theta_{1}+\theta_{2})+0.9cos(\theta_{1})\\
sin(\theta_{1}+\theta_{2})&cos(\theta_{1}+\theta_{2})&0&sin(\theta_{1}+\theta_{2})+0.9sin(\theta_{1})\\
0&0&1&d_{3}+2.1\\
0&0&0&1
\end{bmatrix}
std::double_t pose[4][4] =
{
{{cos(joint_states[1] + joint_states[0])},{-sin(joint_states[1] + joint_states[0])},{(0)},{cos(joint_states[1] + joint_states[0]) + (9 * cos(joint_states[0])) / 10},},
{{sin(joint_states[1] + joint_states[0])},{cos(joint_states[1] + joint_states[0])},{(0)},{sin(joint_states[1] + joint_states[0]) + (9 * sin(joint_states[0])) / 10},},
{{(0)},{(0)},{(1)},{2.1 - joint_states[2]},},
{{(0)},{(0)},{(0)},{(1)},}
};
A variable is created in Float64MultiArray
format which is a 1D-vector with the information of how it should be structured to act as a multidimensional array. It is done by defining the width
and height
of the array as shown
std_msgs::msg::Float64MultiArray message;
message.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
message.layout.dim[0].label = "width";
message.layout.dim[0].size = 4;
message.layout.dim[0].stride = 4 * 4;
message.layout.dim[1].label = "height";
message.layout.dim[1].size = 4;
message.layout.dim[1].stride = 4;
message.layout.data_offset = 0;
In order to remove any garbage data, we first fill the message variable with null value.
message.data.clear();
We flatten the pose variable to be copied into the publishing variable and copy the data
std::vector<double_t> vec(16, 0);
for (size_t i = 0; i < 4; i++)
for (size_t j = 0; j < 4; j++)
vec[(i * 4) + j] = pose[i][j];
message.data = vec;
The data is published on the topic as set before.
RCLCPP_INFO(this->get_logger(), "Publishing...");
fkin_publisher_->publish(message);
The task of the service file is to define the input and output data type of the service.
Here we take $x,y,z$ as input and return $q_{11},q_{12},q_{13},q_{21},q_{22},q_{23}$ as output. Both are of type float64
. The input and output is seperated by ---
between them.
float64 x
float64 y
float64 z
---
float64 q11
float64 q21
float64 q31
float64 q12
float64 q22
float64 q32
Including with essential headers.
#include "custom_interfaces/srv/find_joint_states.hpp"
#include "rclcpp/rclcpp.hpp"
#include <bits/stdc++.h>
#include <math.h>
#include <memory>
The node with name inverse_kinematics_server
is generated and the service with name inverse_kinematics
is attached to it.
The service calls the void add(...)
function when it is initiated.
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("inverse_kinematics_server");
rclcpp::Service<custom_interfaces::srv::FindJointStates>::SharedPtr service = node->create_service<custom_interfaces::srv::FindJointStates>("inverse_kinematics", &add);
We round the input $x,y,z$ coordinates to 2 decimal places first and store it in variables.
std::double_t x = round(request->x * 100) / 100; // x component of the end effector pose
std::double_t y = round(request->y * 100) / 100; // y component of the end effector pose
std::double_t z = round(request->z * 100) / 100; // z component of the end effector pose
We check for the values of $x,y,z$ where robot cannot reach or will face singularity. We quit the node if it is not possible to calculate the solution.
if (std::sqrt((x * x) + (y * y)) > 2)
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "XY Values out of Bound");
solution_possible = false;
}
if (x == 0 && y == 0)
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "XY Values not reachable. Detected Singularity");
solution_possible = false;
}
if (z > 2.1 || z < 0)
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Z Value out of Bound");
solution_possible = false;
}
if (!solution_possible)
return;
We define the Robot Parameters
std::double_t lb = 2; // length of the base of the robot
std::double_t l1 = 0.9; // length of first link
std::double_t l2 = 1; // length of second link
std::double_t l3 = 2.2; // length of third link
We calculate the Joint Position using Geometrical Approach. This is done using the lambda function in cpp
std::double_t q1_1, q1_2, q2_1, q2_2;
std::double_t q3 = lb + 0.1 - z; // prismatic joint displacement
double cos_of_q2 = (pow(x, 2) + pow(y, 2) - pow(l1, 2) - pow(l2, 2)) / (2 * l1 * l2);
auto get_q2 = [=](bool is_positive, double cos_of_q2)
{ return atan2((is_positive ? 1 : -1) * sqrt(1 - pow(cos_of_q2, 2)), cos_of_q2); };
q2_1 = get_q2(true, cos_of_q2);
q2_2 = get_q2(false, cos_of_q2);
auto get_q1 = [=](double q2)
{ return atan2(y, x) - atan2(l2 * sin(q2), l1 + (l2 * cos(q2))); };
q1_1 = get_q1(q2_1);
q1_2 = get_q1(q2_2);
The calculated Joint Position is rounded to 2 decimal places before submitting the response
// allocating the joint values to the server response
response->q11 = round(q1_1 * 100) / 100;
response->q21 = round(q2_1 * 100) / 100;
response->q31 = round(q3 * 100) / 100;
response->q12 = round(q1_2 * 100) / 100;
response->q22 = round(q2_2 * 100) / 100;
response->q32 = round(q3 * 100) / 100;
The task of the service file is to define the input and output data type of the service.
Here we take $q_{1},q_{2},q_{3}$ as input of type float64
. The input and output is seperated by ---
between them. Since we don't have any return for this service file, the output end is kept empty. The output is displayed directly in the terminal live as the position is being achieved.
float64 rq1
float64 rq2
float64 rq3
---
Include the required header files.
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <math.h> /* round, floor, ceil, trunc */
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "custom_interfaces/srv/set_joint_states.hpp"
Initialize the joint_state_controller node first.
rclcpp::init(argc, argv);
auto node = std::make_shared<joint_state_controller>();
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting Joint Effort Control.");
Switch the Gazebo Robot from Position Controller to Effort Controller.
Position Controller: Takes joint positions as input and calculate the efforts on its own to reach the desired position.
Effort Controller: Takes efforts as input and directly applies it to the Robot Joints.
system("ros2 run rrbot_gazebo switch_eff");
system("ros2 topic pub --once /forward_effort_controller/commands std_msgs/msg/Float64MultiArray 'data: [0,0,0]'");
rclcpp::spin(node);
The following components are created for our usage.
service_
: Generate a service with name joint_state_controller
to recieve Target Joint Position from User. The service calls the recieve_reference_joint_position_from_service(...)
function everytime user sends the value via service call.joint_state_subscriber
: Subscriber to read the joint states (position and velocity) published by the gazebo robot on /joint_states
topic. The subscriber is binded to void calculate_joint_efforts(...)
function which is called everytime the subscriber recieves joint states.efforts_publisher
: Publishes the calculated joint efforts to the topic /forward_effort_controller/commands
so the gazebo robot can implement it.reference_value_publisher
: Takes the reference joint values from service_
and publishes it on topic /reference_joint_states/commands
so that it can be used to plot on graph.service_ = this->create_service<custom_interfaces::srv::SetJointStates>("joint_state_controller", std::bind(&joint_state_controller::recieve_reference_joint_position_from_service, this, _1));
joint_state_subscriber_ = create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10, std::bind(&joint_state_controller::calculate_joint_efforts, this, _1));
efforts_publisher_ = create_publisher<std_msgs::msg::Float64MultiArray>("/forward_effort_controller/commands", 10);
reference_value_publisher_ = create_publisher<std_msgs::msg::Float64MultiArray>("/reference_joint_states/commands", 10);
This function is called when the user initiates a service call for a robot to reach a certain position. We have a constraint on Joint 3 because it is a prismatic joint. The prismatic joint cannot be extended more than $2.1m$ and be less than $0m$. Joint 1 and Joint 2 being revolute, have no such constraints and can rotate infintely.
If the recieved positions are within constraints, they are stored in reference_position
variable.
The command_received
variable is switched to true
when service call is made.
void recieve_reference_joint_position_from_service(const std::shared_ptr<custom_interfaces::srv::SetJointStates::Request> request)
{
command_received_ = true;
if (request->rq3 > 2.1 || request->rq3 < 0)
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Joint State 3 not reachable");
return;
}
reference_position = {request->rq1,request->rq2,request->rq3};
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Request (q1,q2,q3): ('%f','%f','%f')", reference_position[0], reference_position[1], reference_position[2]);
}
calculate_joint_efforts
function though called everytime joint_state_subscriber
recieves the joint states, yet it is only effective once the command_received
variable changes to true
.
🤔 I Know! There are much better solution that the one we have implemented. But yeah!! It is what it is! We couldn't think of a smarter solution at 3 AM in the morning!!
We store the current joint position, velocity and calculate error in position.
$$ e = \theta_{r} - \theta $$
Our joint motion direction are switched so we are using the inverted directions.
std::vector<std::double_t> joint_position = {msg->position[0],msg->position[1],msg->position[2]};
std::vector<std::double_t> joint_velocity = {msg->velocity[0],msg->velocity[1],msg->velocity[2]};
std::vector<std::double_t> error = {joint_position[0] - reference_position[0],joint_position[1] - reference_position[1],joint_position[2] - reference_position[2]};
Initial efforts are:
$$\tau_{1} = 0 N$$
$$\tau_{2} = 0 N$$
$$\tau_{3} = 9.8 N$$
$\tau_{3}$ is kept at 9.8N so that the prismatic joint can hold its position when the desired position is achieved. This is due to the fact that the object has mass of 1kg.
apply_joint_efforts = {0, 0, 9.8};
The values of applied torque is changed only if the current error is greater than the acceptable error.
if (abs(error[0]) > acceptable_error) // Joint 1
apply_joint_efforts[0] = -(proportional_gain[0] * error[0]) - (derivative_gain[0] * joint_velocity[0]);
if (abs(error[1]) > acceptable_error) // Joint 2
apply_joint_efforts[1] = -(proportional_gain[1] * error[1]) - (derivative_gain[1] * joint_velocity[1]);
if (abs(error[2]) > acceptable_error) // Joint 3
apply_joint_efforts[2] = -(proportional_gain[2] * error[2]) - (derivative_gain[2] * joint_velocity[2]);
A Float64MultiArray
type variable is created to store the applied torque values for publishing it.
std_msgs::msg::Float64MultiArray message;
message.data.clear();
message.data = apply_joint_efforts;
The recieved joint states from the service is copied in a variable to publish it via the reference states publisher.
std_msgs::msg::Float64MultiArray reference_joint_states;
reference_joint_states.data = reference_position;
The Control efforts and reference joint states are finally published to their respective topics.
reference_value_publisher_->publish(reference_joint_states);
efforts_publisher_->publish(message);
Design Assumptions:
The tuning gains are high for Joint 3 primarily due to the fact that the Joint 3 is prismatic and along the direction of gravitational force causing it to need additional force to counteract the gravity. Opposite to it, Joint 1 and Joint 2 are revolute and working in direction perpendicular to gravity, thus, being able to operate with lowered applied torque.
std::double_t acceptable_error = 0.0001f;
std::vector<std::double_t> proportional_gain = {15, 9.5, 491};
std::vector<std::double_t> derivative_gain = {17, 9.5, 60};
When user presses Ctrl+C
to close the node, a final command is sent so that the applied torques for all controllers can be set to zeros. This allows for any other node to be capable of controlling the robot along with clearing the topic of /forward_effort_controller/commands
which won't spam the topic unnecessarily.
rclcpp::shutdown();
system("ros2 topic pub --once /forward_effort_controller/commands std_msgs/msg/Float64MultiArray 'data: [0,0,0]'");
🥳 Ladies and Gentlemen!! 🥳 Hope you have a good day!! 🥳 We are all done with the project here!!
This project is licensed under GNU General Public License v3.0 (see LICENSE.md).
Copyright 2022 Parth Patel
Licensed under the GNU General Public License, Version 3.0 (the "License"); you may not use this file except in compliance with the License.
You may obtain a copy of the License at
https://www.gnu.org/licenses/gpl-3.0.en.html
Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.