Tutorial CSDA10F Interface

In order to facilitate the control of the robot and its gripper a custom class has been created specifically for the CSDA10F.

CSDA10F class

This class, declared in the invite_utils/include/csda10f_interface.h and implemented in invite_utils/src/csda10f_interface.cpp, provides easy initialization of various useful classes, services, and variables for developing applications with Invite's CSDA10F robot and its grippers.

Upon initialization, the class does the following:

• Initialize and configures all move_groups of the robot for operation
• Verifies that both grippers are connected and that their action services are ready.
• Configures a planning scene monitor and a planning scene interface for easy interaction with the simulated environment.
• Configures an instance of moveit_visual_tools, to provide easy publishing of visual debugging information.
• Starts monitoring for the CSDA10F robot state in the form of industrial_msgs::RobotStatus messages.
• Enables the servomotors of the robot. (can be achieved also by calling the service robot_enable)
• Creates a unified instance of RobotModel and RobotState pointing to the current state of the robot provided by the YASKAWA controller.
• Configures the voice synthesizer capability, to allow the robot to "talk".
• Force the publishing of the current collision objects in the queue, to avoid collision with known static objects. (If the path on the parameter csda10f/collision_scene is found, a collision object planning scene will be loaded and published)

Instantiation

#include <invite_utils/csda10f_interface.h>

...

float max_robot_speed = 0.5;   // From 0 to 1.0 meaning % of absolute joint speed
invite_utils::CSDA10F csda10f(max_robot_speed);

Enable or disable servomotors power

csda10f.enable_robot();
...
csda10f.disable_robot();

Stop robot motion on command

csda10f.stopMotion();

Access the robot move_groups

csda10f.csda10f_mg;                   // 15 DoF
csda10f.arms_mg;              
csda10f.left_arm_with_torso_mg;
csda10f.right_arm_with_torso_mg;
csda10f.left_arm_mg;
csda10f.right_arm_mg;
csda10f.torso_mg;

Get the current state of the robot

csda10f.current_robot_state_ptr // [robot_state::RobotStatePtr]

Operate Robot Grippers

// Open grippers 
csda10f.right_gripper->open();
csda10f.left_gripper->open();

// Close grippers
csda10f.right_gripper->close();
csda10f.left_gripper->close();

// Go to specific position 
csda10f.right_gripper->goToPosition( <position>, <speed>, <force>, <wait_for_completition>);
csda10f.left_gripper->goToPosition( <position>, <speed>, <force>, <wait_for_completition>);

// Get trajectory message for the grippers given a certain distance in between the fingers (usefull for grasping and placing messages)
csda10f.getLeftGripperPosture( <distance_between_fingers> )   # return type: `trajectory_msgs::JointTrajectory`
csda10f.getRightGripperPosture( <distance_between_fingers> )  # return type: `trajectory_msgs::JointTrajectory`

Tare force sensor meassurments (Set current force as reference force or 0)

csda10f.tareLeftForceSensor()