[ad_1]
Half 2 of a collection of articles centered on ROS 2 and the event we made at AICA to develop a Job and Movement Planning (TAMP) framework
Welcome again to this collection of articles centered on the event we made at AICA to develop a Job and Movement Planning (TAMP) framework, extending from ROS 2 options.
Within the earlier article, we launched dynamic composition and its present implementation in ROS 2. On this article, we are going to see the design steps we took to develop a TAMP framework. We are going to focus right here on the movement planning half by showcasing the library we use internally to symbolize robots and object states. We consider in open supply code. Therefore, all of the libraries we listing listed below are publicly accessible.
To plan dynamic robotic actions, the robotic must have correct details about its personal inside state, in addition to the state of its surroundings. All the pieces might be represented as a bodily shifting physique noticed from a sure level, thought of fastened from the observer’s standpoint.
In robotics and physics, the Cartesian state of an object is commonly described as a reference body. A reference body, or coordinate system, is a set of axes used to measure the place and orientation of an object. The selection of reference body can have an effect on the Cartesian state of the thing. For example, the thing’s place and orientation shall be totally different when measured from the angle of an observer on the bottom versus an observer in a shifting automobile.
The item’s orientation is commonly expressed utilizing a quaternion, a mathematical assemble representing a rotation in three-dimensional area. Quaternions are most popular over different representations like Euler angles as a result of they keep away from points like gimbal lock and supply a easy interpolation between orientations. One vital facet is that almost all fashionable libraries present a translation perform between the widespread representations.
The Cartesian state also can embody the linear and angular velocities, known as twist, and the thing’s linear and angular accelerations. These symbolize the charges of change of the linear and angular velocities, respectively. These portions are vital for understanding the dynamics of the thing.
Along with place, orientation, velocities, and accelerations, the Cartesian state also can embody pressure and torque. Power is a vector amount that represents the entire pressure appearing on the thing, whereas torque is a vector amount that represents the entire torque (or second) appearing on the thing. The mixed illustration of these two parts is known as a wrench.
ROS 2 doesn’t present a unified illustration for Cartesian states. As an alternative, the geometry_msgs bundle accommodates a number of messages that may be grouped to symbolize a full Cartesian state. Right here is an instance of learn how to symbolize a Cartesian state in ROS 2:
#embody "geometry_msgs/msg/pose_stamped.hpp"
#embody "geometry_msgs/msg/twist_stamped.hpp"
#embody "geometry_msgs/msg/accel_stamped.hpp"
#embody "geometry_msgs/msg/wrench_stamped.hpp"// Create a PoseStamped message to symbolize the place and orientation
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "world";
pose.pose.place.x = 1.0;
pose.pose.place.y = 2.0;
pose.pose.place.z = 3.0;
pose.pose.orientation = tf2::toMsg(tf2::Quaternion(0.0, 0.0, M_PI/2));
// Create a TwistStamped message to symbolize the linear and angular velocity
geometry_msgs::msg::TwistStamped twist;
twist.header.frame_id = "world";
twist.twist.linear.x = 0.1;
twist.twist.linear.y = 0.2;
twist.twist.linear.z = 0.3;
twist.twist.angular.x = 0.01;
twist.twist.angular.y = 0.02;
twist.twist.angular.z = 0.03;
// Do the identical for AccelStamped and WrenchStamped messages
You’ll then have to create a customized message that teams these 4 messages in a single construction. It turns into clear that it is a very tedious and lengthy course of. Furthermore, extra compact representations can be found utilizing vectors or quaternions generally accessible. The Eigen library, for instance, is an effective way of representing Cartesian states and expressing transformations between them. Lastly, other than having a compact illustration of Cartesian states, the principle benefit is contemplating mathematical operations corresponding to inverting, scaling, and altering reference body…
Specifically, altering the reference body of a Cartesian state entails reworking the state from one coordinate system to a different. This can be a widespread operation in robotics, as totally different system components could use totally different reference frames.
For instance, contemplate a robotic arm that’s choosing up an object. The place and orientation of the thing could be described within the reference body of the robotic’s base. Nonetheless, the management system for the robotic’s gripper would possibly use a special reference body centered on the gripper itself. To manage the gripper precisely, we should remodel the thing’s Cartesian state from the bottom body to the gripper body.
This transformation entails two steps: a translation to account for the distinction in place between the 2 frames and a rotation to account for the distinction in orientation.
In ROS 2, these transformations might be carried out utilizing the tf2 library, which offers instruments for managing a number of coordinate frames and reworking knowledge between them. Right here is an easy instance:
#embody "tf2_geometry_msgs/tf2_geometry_msgs.h"// Assume now we have a PoseStamped msg representing the thing's state within the base body
geometry_msgs::msg::PoseStamped object_state_base;
// And a TransformStamped msg representing the transformation from the bottom body to the gripper body
geometry_msgs::msg::TransformStamped base_to_gripper;
// We are able to use tf2 to remodel the thing's state to the gripper body
geometry_msgs::msg::PoseStamped object_state_gripper;
tf2::doTransform(object_state_base, object_state_gripper, base_to_gripper);
Though this works properly for pose (place and orientation), no such instruments can be found for velocities, accelerations, and wrenches. This motivated us to offer a transparent construction to deal with Cartesian states (and different kinds of states) and the transformations between them. Introducing the state_representation library.
state_representation is the primary library we developed and made publicly accessible as a part of the control-libraries meta-package. It consists of a number of representations of states to elucidate environmental modifications (Cartesian, joint state illustration of robots, objects shapes, …).
The CartesianState class represents a spatial body in 3D area containing the next spatial and dynamic properties:
- Place
- Orientation
- Linear velocity
- Angular velocity
- Linear acceleration
- Angular acceleration
- Power
- Torque
Every state variable is represented as a 3D vector (Eigen::Vector3D), aside from the orientation, which is represented by a unit quaternion (Eigen::Quaterniond). The values are assumed to be in customary SI items (meters, radians, seconds, and Newtons).
The spatial properties are expressed relative to a named reference body. For a CartesianState with the identify “A” and reference body “B,” every state variable represents the instantaneous spatial property measured at or round body “A” from the angle of body “B.” Right here is how one can initialize a CartesianState:
#embody "state_representation/area/cartesian/CartesianState.hpp"// Create a CartesianState A expressed in reference body B
state_representation::CartesianState state("A", "B");
// Set the place
state.set_position(Eigen::Vector3d(1.0, 2.0, 3.0));
// Set the orientation utilizing a quaternion
Eigen::Quaterniond orientation(1.0, 0.0, 0.0, 0.0); // identification quaternion
state.set_orientation(orientation);
// Set the linear velocity
state.set_linear_velocity(Eigen::Vector3d(0.1, 0.2, 0.3));
// Set the angular velocity
state.set_angular_velocity(Eigen::Vector3d(0.01, 0.02, 0.03));
// Do the identical for linear & angular accelerations, pressure and torque
state_representation additionally offers mathematical operations for altering reference frames. To that extent, now we have determined to override the * operator as that is the most typical and compact means of representing chained transformations:
// Assume now we have a CartesianState 'state_in_A' expressed in body 'A'
state_representation::CartesianState state_in_A("state", "A");// And now we have a remodel from body 'A' to a brand new body 'B'
state_representation::CartesianPose A_in_B("A", "B");
// We are able to change the reference body of 'state_in_A' to border 'B'
state_representation::CartesianState state_in_B = state_in_A * A_in_B;
This operation impacts the pose (place and orientation) and all the opposite state variables (twist, acceleration, and wrench). Different operations, corresponding to addition or distinction, are additionally carried out, as proven beneath:
#embody "state_representation/area/cartesian/CartesianState.hpp"// Create two CartesianStates
state_representation::CartesianState state1("state1", "world");
state_representation::CartesianState state2("state2", "world");
// Set their positions
state1.set_position(Eigen::Vector3d(1.0, 2.0, 3.0));
state2.set_position(Eigen::Vector3d(4.0, 5.0, 6.0));
// Add the states
state_representation::CartesianState sum = state1 + state2;
// Subtract the states
state_representation::CartesianState distinction = state1 - state2;
These operations are checked for validity. For instance, including states not expressed in the identical reference body will throw an error. We refer the reader to the documentation for extra functionalities, together with derived class to precise single state variables, time derivation and integration, and norms. Please, word this library is offered each in C++ and Python.
One vital facet of robotics lies within the existence of two distinct areas. As seen above, the Cartesian area represents the state of shifting our bodies, such because the robotic’s end-effector or base. Conversely, the joint state represents the state of the joints of a robotic. It usually consists of the joint positions (angles for revolute joints or displacements for prismatic joints), velocities (angular or linear), and accelerations. It might additionally embody joint torques or forces.
Following the very same sample and API of CartesianState, state_representation present a JointState class for representing joint states:
#embody "state_representation/robotic/JointState.hpp"// Create a JointState for a robotic with 7 joints
state_representation::JointState joint_state("my_robot", 7);
// Set the joint positions
joint_state.set_positions(Eigen::VectorXd::Random(7));
// Set the joint velocities
joint_state.set_velocities(Eigen::VectorXd::Random(7));
// Set the joint torques
joint_state.set_torques(Eigen::VectorXd::Random(7));
On this code, we create a JointState for a robotic with seven joints, and set the joint positions, velocities, and torques to random values. The accelerations are set to zeroes that are the default values.
One elementary idea of robotics is the power to remodel Cartesian area into joint area and vice versa. Realizing the robotic’s state, one can infer the Cartesian state of every robotic’s physique half. This may be achieved for all state variables of each states.
Kinematics is the examine of movement with out contemplating the forces that trigger the movement. In robotics, kinematics entails the mathematical description of the movement of robots. There are two main kinds of kinematics: direct (or ahead) kinematics and inverse kinematics.
However, dynamics entails learning forces and torques that trigger movement in robots. Much like kinematics, dynamics might be divided into two varieties: direct (or ahead) dynamics and inverse dynamics.
Direct (ahead) kinematics
Direct kinematics is the method of figuring out the place and orientation of the end-effector (the robotic’s device or hand) given the robotic’s joint angles and lengths. In different phrases, if you understand the angles of all of the joints within the robotic, ahead kinematics lets you calculate the place the end-effector is.
The mathematical mannequin that describes this relationship is commonly represented as a perform that maps joint variables (angles for revolute joints, displacements for prismatic joints) to a pose in Cartesian area (place and orientation).
Inverse kinematics
Inverse kinematics is the reverse means of direct kinematics. Given a desired pose of the end-effector, inverse kinematics determines the joint angles and lengths that may obtain that place. In different phrases, if you need the robotic to achieve a sure location, inverse kinematics tells you learn how to place and orient every robotic’s joints.
Inverse kinematics is commonly extra advanced than direct kinematics as a result of there could also be a number of units of joint angles that end in the identical end-effector place (a number of options), or there could should be a solution to place the joints to achieve the specified location (no answer).
Each direct and inverse kinematics are elementary to robotic movement planning and management. Direct kinematics is commonly utilized in robotic simulation and visualization, whereas inverse kinematics is utilized in path planning and management, the place the objective is to maneuver the end-effector to a desired location.
Direct (ahead) dynamics
Direct dynamics is the method of figuring out the robotic’s acceleration given the joint torques (or forces for prismatic joints) and the present state of the robotic (joint positions and velocities). In different phrases, if you understand the forces utilized to the robotic and its present state, ahead dynamics permit you to calculate how the robotic will transfer.
The mathematical mannequin that describes this relationship is commonly represented as a perform that maps joint torques and the present state to joint accelerations. This perform is usually derived from the robotic’s equations of movement, that are based mostly on Newton’s second regulation of movement (pressure equals mass occasions acceleration).
Inverse dynamics
Inverse dynamics is the reverse means of direct dynamics. Given the specified movement of the robotic (joint positions, velocities, and accelerations), inverse dynamics determines the joint torques that may obtain that movement. In different phrases, if you need the robotic to maneuver a sure means, inverse dynamics tells you ways a lot pressure or torque to use at every joint.
Inverse dynamics is commonly extra advanced than direct dynamics as a result of it entails calculating the forces and torques required to beat inertia, gravity, and damping. Nonetheless, it’s a essential a part of robotic management, because it permits the management system to command the suitable forces and torques to realize the specified movement.
Each direct and inverse dynamics are elementary to robotic movement planning and management. Direct dynamics is commonly utilized in robotic simulation, the place the objective is to foretell how the robotic will transfer given sure forces. Inverse dynamics is utilized in robotic management, the place the objective is to command the robotic to maneuver in a sure means.
Utilizing Pinocchio
To compute each kinematics and dynamics, we depend on the open-source library pinocchio. It’s written in C++ and is designed to be quick and versatile. To get data on the robotic (section size, physique lots, and so on.), the library depends on URDF fashions generally utilized in ROS / ROS 2.
We now have developed a wrapper round it within the robot_model library. This library is absolutely built-in with CartesianState and JointState from state_representation. We use it to transform areas whereas we develop controllers.
For instance, it offers simple-to-use features for ahead and inverse kinematics:
// initialize the robotic mannequin from a URDF
robot_model::Mannequin mannequin("myrobot", "path/to/urdf/myrobot.urdf")// ahead kinematics: pose of end-effector body from the robotic joint positions
state_representation::JointPositions jp = state_representation::JointPositions::Random("myrobot", 7);
state_representation::CartesianPose pose = mannequin.forward_kinematics(jp);
// inverse kinematics: joint positions from end-effector pose
state_representation::CartesianPose cp = state_representation::CartesianPose::Random("eef");
state_representation::JointPositions jp = mannequin.inverse_kinematics(cp);
// ahead velocity kinematics: twist of end-effector body from the robotic joint velocities and positions
state_representation::JointState js = state_representation::JointState::Random("myrobot", 7);
state_representation::CartesianTwist twist = mannequin.forward_velocity(js);
// inverse velocity kinematics: joint velocities from end-effector twist and present state of the robotic
state_representation::CartesianTwist ct = state_representation::CartesianTwist::Random("eef");
state_representation::JointPositions jp = state_representation::JointPositions::Random("myrobot", 7);
state_representation::JointVelocities jv = mannequin.inverse_velocity(ct, jp);
and in addition ahead and inverse dynamics:
// inertia matrix from joint positions
state_representation::JointPositions jp = state_representation::JointPositions::Random("myrobot", 7);
Eigen::MatrixXd inertia = mannequin.compute_inertia_matrix(jp);
// inertia torques, equal to inertia multiplied by joint accelerations. The joint positions a part of the state
// is used to compute the inertia matrix
state_representation::JointState js = state_representation::JointState::Random("myrobot", 7);
state_representation::JointTorques inertia_t = mannequin.compute_inertia_torques(js);// coriolis matrix from joint positions and velocities
state_representation::JointState js = state_representation::JointState::Random("myrobot", 7);
Eigen::MatrixXd coriolis = mannequin.compute_coriolis_matrix(js);
// coriolis torques, equal to coriolis multiplied by joint velocities. The joint positions a part of the state
// is used to compute the coriolis matrix
state_representation::JointState js = state_representation::JointState::Random("myrobot", 7);
state_representation::JointTorques coriolis_t = mannequin.compute_coriolis_torques(js);
// gravity torques from joint positions
state_representation::JointPositions jp = state_representation::JointPositions::Random("myrobot", 7);
state_representation::JointTorques gravity_t = mannequin.compute_gravity_torques(jp);
Though dynamics computations can be found, these will not be used as a lot as kinematics in follow. One of many principal causes is that robotic producers deal with inertia and mass knowledge as commerce secrets and techniques; even when they supply them, they’re typically inaccurate. Nonetheless, there are new methods to study these knowledge from direct interplay. We’re this to enhance these fashions and obtain a finer stage of management for force-based purposes.
On this article, now we have seen how the state of the robotic and its surroundings might be represented utilizing Cartesian and joint states. We now have hinted at how each areas might be reworked and their relationships. We additionally launched the libraries we developed at AICA to deal with these transformations.
The state_representation and robot_model libraries are designed to be extremely versatile and simple to make use of. They prolong the illustration of states that may be present in ROS / ROS 2 and depend on fashionable and environment friendly libraries for computation (corresponding to Eigen and pinocchio). Each libraries might be put in from the control-libraries meta-package and supply C++ and Python implementations.
Taking a look at control-libraries, you’ll discover two extra libraries, dynamical_systems and controllers that weren’t addressed on this article. This would be the focus of the following article, which can cowl the subjects of movement planning and management methods based mostly on the illustration of states launched right here. Keep tuned for extra insights on robotics!
[ad_2]
