ROS2 Interface for the CCA Planner

This repository provides robot-agnostic ROS2 packages that interface the Closed-Chain Affordance(CCA) planner with robotic systems. The CCA planner offers an intuitive approach to planning joint trajectories for robot manipulation tasks that can be thought of as linear, rotational, or screw motions. Defining a task is as simple as specifying an axis, location, and pitch (if applicable). Additionally, it provides the capability to control the end-effector's orientation along the task path. The demonstration video of the underlying paper showcasing simulation and real-world tasks is available here. An interactive RViz plugin is also available as a way to interface with the planner. See the plugin demo videos for a glance at the framework's capabilities.

Requirements

• C++20
• ROS Humble

Dependencies

Core Dependency:

• CCA planner - Install from here.

ROS Dependencies

• moveit: For self-collision checking
• moveit_visual_tools: For visualization of joint movement
• behaviortree_cpp: To utilize the CCA Behavior Tree action node

With ROS sourced, you may install the ROS dependencies with:

sudo apt install ros-${ROS_DISTRO}-moveit ros-${ROS_DISTRO}-moveit-visual-tools ros-${ROS_DISTRO}-behaviortree-cpp

Build Instructions

1. Clone the packages into your ROS2 workspace's src folder, for example:

   mkdir -p ~/ws_cca_ros/src && cd ~/ws_cca_ros/src
   git clone git@github.com:UTNuclearRoboticsPublic/closed_chain_affordance_ros.git

2. Build and source the workspace:

   cd ~/ws_cca_ros
   colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
   source install/setup.bash

Readily-Supported Robots

An additional cca_<robot> package containing robot-specific information is required to launch the planner for a particular robot. Packages are currently available for the following robots, with links provided below. Creating a package for a new robot is simple, quick, and largely automated, as discussed in the Implementing the Framework on a New Robot section.

• Boston Dynamics Spot robot
• Kinova Gen3 7DoF arm

Usage

There are four ways to interact with the CCA planner through ROS2:

1. Programmatic Planning and Execution: Directly call the CCA planner from within a ROS2 node (e.g., the autogenerated cca_<robot>_node.cpp). See the Code Tutorial section for details.
2. Behavior Tree Integration: Use the CCA planner as a node within a Behavior Tree. See cca_ros_behavior package for details. Xml usage is as follows:

   <!-- for a single request, req, a shared ptr to a planning request -->
   <CcaRosAction cca_planning_request="{req}"/>
   <!-- or for multiple requests, reqs, a shared ptr to a vector of planning requests -->
   <CcaRosAction cca_planning_requests="{reqs}"/>

3. Action Server: Use the CCA action server to send planning requests and receive planned trajectories. See cca_ros_action package for details.
4. Rviz Plugin: A user-friendly Rviz plugin is also available and enables visual trajectory planning and execution by simply dragging interactive markers and specifying task types and goals. Launch instructions are provided in the Interactive Rviz Plugin Planning section.

Implementing the Framework on a New Robot

Creating the cca_<robot> Package

1. Use the package creator script:

   cd ~/ws_cca_ros/src/closed_chain_affordance_ros
   ./cca_robot_package_creator.sh

2. Configure the generated package:

   • Complete the config/cca_<robot>_description.yaml file, which contains information about the robot kinematic chain, planning group, relevant action servers, etc.

   • Complete the launch/cca_<robot>_settings.py module by providing information about how to generate robot description and robot description semantic parameters. Inline comments provide detailed guidance on filling out these files.

   • For programmatic trajectory planning and execution, implement task (affordance) details in cca_<robot>_node.cpp. Alternatively, you may use the Rviz plugin for interactive planning.

3. Build the new package:

   cd ~/<ros_workspace_name>
   colcon build --packages-select cca_<robot> --cmake-args -DCMAKE_BUILD_TYPE=Release
   source install/setup.bash

Running the Planner

Prerequisites

• For real-robot execution, ensure a follow_joint_trajectory action server is active, joint_states are being published, and TF data is available.
  To plan without a physical robot, simply provide joint_states and TF data.

Programmatic Trajectory Planning

1. Launch trajectory validation and visualization server:

   ros2 launch cca_<robot> cca_<robot>_val_and_viz.launch.py launch_rviz:=true

2. Run the planner for tasks defined in cca_<robot>_node.cpp:

   ros2 launch cca_<robot> cca_<robot>.launch.py

Interactive Rviz Plugin Planning

1. Start the CCA ROS action server:

   ros2 launch cca_<robot> cca_<robot>_action_server.launch.py

2. Launch Rviz with the interactive planning plugin:

   ros2 launch cca_<robot> cca_<robot>_val_and_viz.launch.py launch_rviz:=true

Code Tutorial

Here, we provide a brief tutorial on how to use the CCA planner programmatically within a ROS2 node.

Planning Request(s)

We use a request–response model to communicate with the planner.
A typical request is structured as follows:

cca_ros::PlanningRequest req;
req.task_description = // See examples below in the "Task Description" section
req.planning_group = // Name of the planning group for which to plan, for example "arm"
req.execute_trajectory = true; // Set to true to execute the trajectory on the robot (default: false)
req.time_step = // Time interval (in seconds) between consecutive trajectory points, for example req.time_step.robot = 0.1;

Optional / Advanced Settings

These are rarely needed in typical usage:

req.planner_config = // Custom planner configuration (default settings are usually sufficient). See more info below.
req.start_state = // Specify a starting joint configuration (default: current robot state)

Planning Multiple Tasks at Once

One may also ask the planner to plan for a series of tasks in one go. This produces one long trajectory that accomplishes all tasks sequentially.

std::vector<cca_ros::PlanningRequest> reqs;
// Fill in each req as above
cca_ros::PlanningRequest req1;
reqs.push_back(req1);
cca_ros::PlanningRequest req2;
reqs.push_back(req2);
// and so on...

Calling the Planner

Once the request(s) are defined, call the planner as follows:

// Create the planner node object
auto node = std::make_shared<CcaRos>("cca_ros");

// Spin the node so joint states can be read
std::jthread spinner_thread([node]() { rclcpp::spin(node); });

// Call the planner
cca_ros::PlanningResponse resp = node->plan(req); // For single request
// Or for multiple requests
// cca_ros::PlanningResponse resp = this->plan(reqs);

// Checking response
// Success is indicated by resp.result.success
bool planner_success = resp.result.success;

// For trajectory execution, one can also check planner motion status with
auto motion_status = response.status;
// See CCA ROS Doxygen documentation for details on status codes as well as other fields in the response, 
// which may be of use to your application.

Task Description

Broadly, CCA provides four planning types:

1. Affordance Planning: Plan tasks in terms of elementary motions—linear, rotational, or screw. Affordance describes the type of motion that an object offers as a task. For instance, a valve "affords" turning, which could be modeled as rotational motion.
2. Cartesian Goal Planning: Plan tasks to reach a specified Cartesian goal pose.
3. In-Place EE Orientation Control: Plan to adjust the end effector (EE) orientation about a specific axis without translational movement.
4. Approach Planning: Given an affordance definition and a reference pose, plan to a pose along the affordance path that achieves a specified affordance goal. For example, move from an arbitrary current configuration to a pose that corresponds to 90 degrees along a rotational path defined by the affordance.

Below, we provide task examples for each of these categories as well as various affordance types. Note all vectors are defined in the reference frame specified in the config/cca_<robot>_description.yaml of the cca_<robot> package.

Affordance Tasks

Translation

We want to move 0.8m along the z-axis

// Task instantiation
cc_affordance_planner::TaskDescription task_description(cc_affordance_planner::PlanningType::AFFORDANCE);

// Affordance info
task_description.affordance_info.type = affordance_util::ScrewType::TRANSLATION;
task_description.affordance_info.axis = Eigen::Vector3d(0, 0, 1);
task_description.affordance_info.location = Eigen::Vector3d::Zero();

// Goals
task_description.goal.affordance = 0.8;
// Optional: Constrain the EE to a particular desired orientation along the path
task_description.goal.ee_orientation = Eigen::Vector3d(M_PI/12, M_PI/8, M_PI/12); // as EE-frame rpy

Rotation

We want to rotate about the z-axis by 270degrees

// Task instantiation
cc_affordance_planner::TaskDescription task_description(cc_affordance_planner::PlanningType::AFFORDANCE);

// Affordance info
task_description.affordance_info.type = affordance_util::ScrewType::ROTATION;
task_description.affordance_info.axis = Eigen::Vector3d(0, 0, 1);
task_description.affordance_info.location = Eigen::Vector3d(0.0, 0.0, 0.8);

// Goals
task_description.goal.affordance = 3.0 * M_PI / 2.0;

Screw Motion

We want to perform a screw motion about the negative y-axis by 270degrees at a 0.1m/rad pitch

// Task instantiation
cc_affordance_planner::TaskDescription task_description(cc_affordance_planner::PlanningType::AFFORDANCE);

// Affordance info
task_description.affordance_info.type = affordance_util::ScrewType::SCREW;
task_description.affordance_info.axis = Eigen::Vector3d(0, -1, 0);
task_description.affordance_info.location = Eigen::Vector3d(-0.4, 0, 0.5);
task_description.affordance_info.pitch = 0.1;

// Goals
task_description.goal.affordance = 3.0 * M_PI / 2.0;

In-Place End-Effector Orientation Control Task

While keeping the EE position fixed, we want to change its orientation about the x-axis by 90degrees

// Task instatiation
cc_affordance_planner::TaskDescription task_description(cc_affordance_planner::PlanningType::EE_ORIENTATION_ONLY);

// Affordance info
task_description.affordance_info.axis = Eigen::Vector3d(1, 0, 0);

// Goal
task_description.goal.affordance = M_PI / 2.0;

Cartesian Goal Planning Task

We want to plan to a desired cartesian goal

// Task instatiation
 cc_affordance_planner::TaskDescription task_description(cc_affordance_planner::PlanningType::CARTESIAN_GOAL);

// Goal
task_description.goal.canonical_pose = Eigen::Matrix4d::Identity(); // as a 4x4 Homogeneous Transformation Matrix
task_description.goal.canonical_pose.block<3, 1>(0, 3) =
    (Eigen::Vector3d() << 0.70932, 0.000336774, -0.017661).finished(); // we specify position, but leave orientation as identity for this example.

Approach Motion Planning Task

We want to move from the current configuration to a pose along the affordance path, positioned 90 degrees from the specified reference (canonical) pose.

// Task instatiation
task_description = cc_affordance_planner::TaskDescription(cc_affordance_planner::PlanningType::APPROACH);

// Affordance info
task_description.affordance_info.type = affordance_util::ScrewType::ROTATION;
task_description.affordance_info.axis = Eigen::Vector3d(1, 0, 0);
task_description.affordance_info.location = Eigen::Vector3d::Zero();

// Goal
task_description.goal.affordance = M_PI / 2.0; // Set desired goal for the affordance
task_description.goal.canonical_pose = Eigen::Matrix4d::Identity();
task_description.goal.canonical_pose.block<3, 1>(0, 3) = Eigen::Vector3d(0.3, 0.3, 0.5);

Defining Tasks in Relation to a Frame of Choice

In the following examples, we specify the task in different frames that can be looked up in the TF tree.

Affordance Planning Task

We want to do a 90-degree rotation about the z axis of the EE frame

// Affordance info from -- Gets axis and location 
task_description.affordance_info.from.method = affordance_util::PoseSpecificationMethod::FROM_FRAME_NAME;
task_description.affordance_info.from.frame_name = "ee_frame"; // Specify a valid frame name that appears in the TF tree
task_description.affordance_info.from.axis_in_final_pose = affordance_util::axis_to_vec(affordance_util::Axis::Z); // Choose z-axis of that frame

// Specify type
task_description.affordance_info.type = affordance_util::ScrewType::ROTATION;

// Goal
task_description.goal.affordance = M_PI / 2.0; // Set desired goal for the affordance

We want to do a 90-degree rotation about the z axis of some frame B defined in relation to some frame A

// Affordance info from -- Gets axis and location 
task_description.affordance_info.from.method = affordance_util::PoseSpecificationMethod::FROM_FRAME_NAME;
task_description.affordance_info.from.frame_name = "frame_a"; // Specify a valid frame name that appears in the TF tree

// Define the transform from frame_a to frame_b -- For simple example, just 10cm along the x-axis.
Eigen::Isometry3d T_a_to_b = Eigen::Isometry3d::Identity();
T_a_to_b.translation() = Eigen::Vector3d(0.1, 0.0, 0.0);
task_description.affordance_info.from.post_transform = T_a_to_b.matrix(); 

task_description.affordance_info.from.axis_in_final_pose = affordance_util::axis_to_vec(affordance_util::Axis::Z); // Choose z-axis of frame b

// Specify type
task_description.affordance_info.type = affordance_util::ScrewType::ROTATION;

// Goal
task_description.goal.affordance = M_PI / 2.0; // Set desired goal for the affordance

Cartesian Goal Planning Task

We want to plan to a cartesian pose offset from some frame

// Task instatiation
cc_affordance_planner::TaskDescription task_description(cc_affordance_planner::PlanningType::CARTESIAN_GOAL);

task_description.canonical_pose_from.method = affordance_util::PoseSpecificationMethod::FROM_FRAME_NAME;
task_description.canonical_pose_from.frame_name = "frame_a"; // Specify a valid frame name that appears in the TF tree

// Define the transform from frame_a to frame_b -- 
Eigen::Isometry3d T_a_to_b = Eigen::Isometry3d::Identity();
T_a_to_b.linear() = Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()).toRotationMatrix(); // 90-deg rotation about the x-axis
T_a_to_b.translation() = Eigen::Vector3d(0.1, 0.0, 0.0); // 10cm along the x-axis
task_description.canonical_pose_from.post_transform = T_a_to_b.matrix(); 

Task Description Settings

Occasionally, you may need to consider the following settings for the task:

task_description.trajectory_density = 50; // Require the joint trajectory to have 50 points. By default this is 10

Planner Configuration

And seldom, you may need to touch planner settings

cc_affordance_planner::PlannerConfig plannerConfig;
plannerConfig.ik_max_itr = 1000; // Default is 200. Raise this if the planner seems not to solve joint trajectories you think are feasible.
plannerConfig.accuracy = 1.0/100; // Accuracy of the planner, 1% for example. Default is 10%

We leave the other settings to the advanced reader to persue through the CCA library documentation

Author

Janak Panthi (aka Crasun Jans)