RV Panda Driver
Overview
The Panda Driver provides a convienent ROS interface to the Franka-Emika Panda robotic arm. It provides multiple control modes, and hides the complexity of changing between these modes.
This package extends the RV Manipulation Driver package.
Usage
Launching the driver
The Panda Driver is launched by:
roslaunch rv_panda_driver robot_bringup.launch
When starting up the Panda robot:
- The lights indicate the status of the robot.
- Blue = happy and ready to move
- White = happy but E-stopped
- Yellow = error
- You will need to release the joint locks through the Franka interface
- When operating the arm have the E-stop handy for safety at all times
- When the arm is E-stopped the robot can be moved freely using the wrist switch
To ensure the arm is working once started you can invoke the home service.
rosservice call /arm/home
Moving the arm
The Panda Driver provides a number of options for moving the robot arm. These include moving to fixed poses, as well as moving the arm by specifiying Cartesian velocities for the end-effector.
Moving the End-Effector to an Arbitrary Pose
The following code example moves the robot end-effector to an arbitrary position and orientation w.r.t. to the robot's base frame.
#!/usr/bin/env python2
import rospy
import actionlib
from rv_msgs.msg import MoveToPoseAction, MoveToPoseGoal
from geometry_msgs.msg import PoseStamped
# initialise ros node
rospy.init_node('move_to_points_example')
# Create a ros action client to communicate with the driver
client = actionlib.SimpleActionClient('/arm/cartesian/pose', MoveToPoseAction)
client.wait_for_server()
# Create a target pose
target = PoseStamped()
target.header.frame_id = 'panda_link0'
# Populate with target position/orientation (READY POSE)
target.pose.position.x = 0.307
target.pose.position.y = 0.000
target.pose.position.z = 0.590
target.pose.orientation.x = -1.00
target.pose.orientation.y = 0.00
target.pose.orientation.z = 0.00
target.pose.orientation.w = 0.00
# Create goal from target pose
goal = MoveToPoseGoal(goal_pose=target)
# Send goal and wait for it to finish
client.send_goal(goal)
client.wait_for_result()
Servoing the End-Effector to an Arbitrary Pose
The following code example servos the robot end-effector to an arbitrary position and orientation w.r.t. to the robot's base frame.
#!/usr/bin/env python2
import rospy
import actionlib
from rv_msgs.msg import ServoToPoseAction, ServoToPoseGoal
from geometry_msgs.msg import PoseStamped
# initialise ros node
rospy.init_node('servo_to_points_example')
# Create a ros action client to communicate with the driver
client = actionlib.SimpleActionClient('/arm/cartesian/servo_pose', ServoToPoseAction)
client.wait_for_server()
# Create a target pose
target = PoseStamped()
target.header.frame_id = 'panda_link0'
# Populate with target position/orientation (READY POSE)
target.pose.position.x = 0.307
target.pose.position.y = 0.000
target.pose.position.z = 0.590
target.pose.orientation.x = -1.00
target.pose.orientation.y = 0.00
target.pose.orientation.z = 0.00
target.pose.orientation.w = 0.00
# Create goal from target pose
goal = ServoToPoseGoal(stamped_pose=target, scaling=0.5)
# Send goal and wait for it to finish
client.send_goal(goal)
client.wait_for_result()
Controlling End-Effector Cartesian Velocity
The following code example moves the robot end-effector at 2cm/s in the z-axis w.r.t. robot's base-frame for a total of 5 seconds. For safety reasons the driver has an expected minimum update frequency of 100Hz.
#!/usr/bin/env python2
import rospy
import timeit
from geometry_msgs.msg import TwistStamped
# initialise ros node
rospy.init_node('cartesian_motion')
# Create the publisher (queue size tells ROS to only publish
# the latest message)
publisher = rospy.Publisher('/arm/cartesian/velocity', TwistStamped, queue_size=1)
# Create an initial start time
start_timer = timeit.default_timer()
# Create a velocity message that will instruct the robot to
# move at 2cm a second in the z-axis of the base frame.
velocity = TwistStamped()
velocity.twist.linear.z = 0.02
# Publish the velocity message to the Panda driver at a
# frequency of 100Hz
while (timeit.default_timer() - start_timer) < 5:
publisher.publish(velocity)
rospy.sleep(0.01)
# Publish an empty TwistStamped to ensure that the arm stops moving
publisher.publish(TwistStamped())
Named Joint Configurations
In applications we typically define a number of task specific configurations, and it is convienent if these can be assigned names.
While the MoveIt framework provide the ability to store joint configurations (called named poses), these must be defined in the MoveIt configuration of the robot and cannot be adjusted during operation.
The Panda Driver solves this issue by providing the ability to save and remember joint configurations of the Panda robot for future use, while also providing access to named poses provided by MoveIt.
Saving Named Poses
To save the current joint configuration of the robot with the name test_pose:
rosservice call /arm/set_named_pose "pose_name: 'test_pose' overwrite: false"
Getting Named Poses
To get the set of currently stored named joint configurations (created either through the driver or MoveIt)
rosservice call /arm/get_named_poses
Moving to a Named Pose
To move to a named named joint configuration:
#!/usr/bin/env python2
import rospy
import actionlib
from rv_msgs.msg import MoveToNamedPoseAction, MoveToNamedPoseGoal
# initialise ros node
rospy.init_node('named_pose_example')
# Create a ros action client to communicate with the driver
client = actionlib.SimpleActionClient('/arm/cartesian/named_pose', MoveToNamedPoseAction)
client.wait_for_server()
# Create and send a goal to move to the named pose test_pose
client.send_goal(MoveToNamedPoseGoal(pose_name='test_pose'))
client.wait_for_result()
Note: The goal pose can be a named pose created either by the driver or MoveIt. However, the named poses provided by the driver will take priority in the event of a naming conflict.
Subscribed Topics
-
/arm/cartesian/velocity (geometry_msgs/TwistStamped) Moves the end-effector in Cartesian space w.r.t. the target frame_id (base frame if no frame_id is set).
-
/arm/joint/velocity (rv_msgs/JointVelocity) Moves the joints of the manipulator at the requested velocity.
Published Topics
- /arm/state (rv_msgs/ManipulatorState) Provides information on the current state of the manipulator including the pose of the end-effector w.r.t. to the base link, whether the manipulator is experiencing a Cartesian contact and collision as a bit-wised error state flag.
Services
-
/arm/home (std_srvs/Empty) Moves the robot back to its initial ready pose.
-
/arm/recover (std_srvs/Empty) Recovers from collision or limit violation error states that will put the robot into a non-operable state.
-
/arm/stop (std_srvs/Empty) Stops the current motion of the current.
-
/arm/set_ee_offset (rv_msgs/SetPose) Sets the offset between the end-effector and the wrist of the robot arm.
-
/arm/get_named_poses (rv_msgs/GetNamesList) Gets a list of currently stored named poses (includes both MoveIt and driver stored named poses).
-
/arm/set_named_pose (rv_msgs/SetNamedPose) Saves the current joint configuration of the robot with the provided pose name.
-
/arm/set_cartesian_impedance (rv_msgs/SetCartesianImpedance Adjusts the impedenace of the end-effector position in Cartesian space.
-
/arm/get_link_position (rv_msgs/GetRelativePose) A convenience wrapper around the ROS transform lookup service that provides the relative pose of a target frame w.r.t. a reference frame.
-
/arm/set_cartesian_planning_enabled (std_srvs/SetBool) Forces the path-planner to only consider linear paths when moving moving between poses with /arm/cartesian/pose
-
/arm/get_cartesian_planning_enabled (rv_msgs/SimpleRequest) Checks whether the path-planner will only consider linear paths when moving moving between poses with /arm/cartesian/pose
-
/arm/add_named_pose_config (rv_msgs/SetNamedPoseConfig) Instructs the driver to load named poses stored in the indicated config file.
-
/arm/get_named_pose_configs (rv_msgs/GetNamedPoseConfigs) Gets the list of config files to check for named poses.
-
/arm/remove_named_pose_config (rv_msgs/SetNamedPoseConfig) Instructs the driver to remove named poses stored in the indicated config file.
Action API
-
/arm/cartesian/pose (rv_msgs/MoveToPose.action) Moves the end-effector to the requested goal pose w.r.t. the indicated frame id.
-
/arm/cartesian/servo_pose (rv_msgs/ServoToPose.action) Servos the end-effector to the requested goal pose.
-
/arm/cartesian/named_pose (rv_msgs/MoveToNamedPose.action) Moves the end-effector to a pre-defined joint configuration.
-
/arm/cartesian/servo_pose (rv_msgs/MoveToJointPoseAction) Moves the joints of the robot to the indicated positions (radians).
-
/arm/gripper (rv_msgs/ActuateGripper.action) Actuates the gripper based on the requested mode. The static mode will move the gripper to the requested width. The grasp mode will attempt to grasp an object of width plus/minus a tolernace factor.