Completed the setup for Franka Emika Panda Robot

README.md

@@ -20,6 +20,7 @@ We will try to maintain compatibility with ROS Melodic for as long as possible,
 ## Robots currently implemented
 - MiR100
 - Universal Robots: UR3, UR3e, UR5, UR5e, UR10, UR10e, UR16
+- Franka Emika Panda Robot
 
 <br>
 

panda_robot_server/config/panda_gripper_controller.yaml

@@ -0,0 +1,13 @@
+
+panda_gripper_controller:
+  type: panda_sim_controllers/PandaGripperController
+  pub_topic:  /franka_gripper/joint_states
+  joints:
+        # main joint
+        panda_finger_joint1_controller:
+          joint: panda_finger_joint1
+          pid: {p: 5000,  i: 50, d: 0.5}
+        # mimic joint
+        panda_finger_joint2_controller:
+          joint: panda_finger_joint2
+          pid: {p: 5000,  i: 50, d: 0.5}

panda_robot_server/config/panda_sim_controllers.yaml

@@ -0,0 +1,101 @@
+panda_simulator:
+
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 100
+
+
+  # Panda SDK Controllers: Position --------------------------
+  position_joint_position_controller:
+    type: panda_sim_controllers/PandaPositionController
+    topic_joint_command: /panda_simulator/motion_controller/arm/joint_commands
+    topic_set_speed_ratio: /panda_simulator/arm/set_speed_ratio
+    joints:
+      panda_joint1_controller:
+        joint: panda_joint1
+        pid: {p: 3000,  i: 10.0, d: 25.0}
+      panda_joint2_controller:
+        joint: panda_joint2
+        pid: {p: 3000,  i: 10.0, d: 30.0}
+      panda_joint3_controller:
+        joint: panda_joint3
+        pid: {p: 3000,  i: 10.0, d: 30.0}
+      panda_joint4_controller:
+        joint: panda_joint4
+        pid: {p: 3000,  i: 10.0, d: 50.0}
+      panda_joint5_controller:
+        joint: panda_joint5
+        pid: {p: 3000,  i: 10.0, d: 8.0}
+      panda_joint6_controller:
+        joint: panda_joint6
+        pid: {p: 3000,  i: 1.0, d: 5.0}
+      panda_joint7_controller:
+        joint: panda_joint7
+        pid: {p: 3000,  i: 10.0, d: 4.0}
+
+  # Panda SDK Controllers: Velocity --------------------------
+  velocity_joint_velocity_controller:
+    type: panda_sim_controllers/PandaVelocityController
+    topic: /panda_simulator/motion_controller/arm/joint_commands
+    joints:
+      panda_joint1_controller:
+        joint: panda_joint1
+        pid: {p: 10,  i: 0.0, d: 0.1}
+      panda_joint2_controller:
+        joint: panda_joint2
+        pid: {p: 100,  i: 1.0, d: 0.1}
+      panda_joint3_controller:
+        joint: panda_joint3
+        pid: {p: 0.05,  i: 0.0, d: 0.01}
+      panda_joint4_controller:
+        joint: panda_joint4
+        pid: {p: 0.5,  i: 0.01, d: 0.1}
+      panda_joint5_controller:
+        joint: panda_joint5
+        pid: {p: 1.0,  i: 0.0, d: 0.01}
+      panda_joint6_controller:
+        joint: panda_joint6
+        pid: {p: 0.05,  i: 0.0, d: 0.01}
+      panda_joint7_controller:
+        joint: panda_joint7
+        pid: {p: 0.05,  i: 0.0, d: 0.01}
+
+  # Panda SDK Controllers: Effort --------------------------
+  effort_joint_torque_controller:
+    type: panda_sim_controllers/PandaEffortController
+    topic: /panda_simulator/motion_controller/arm/joint_commands
+    joints:
+      panda_joint1_controller:
+        joint: panda_joint1
+      panda_joint2_controller:
+        joint: panda_joint2
+      panda_joint3_controller:
+        joint: panda_joint3
+      panda_joint4_controller:
+        joint: panda_joint4
+      panda_joint5_controller:
+        joint: panda_joint5
+      panda_joint6_controller:
+        joint: panda_joint6
+      panda_joint7_controller:
+        joint: panda_joint7
+
+  # Panda SDK Controllers: Gravity Compensation ------------
+  effort_joint_gravity_controller:
+    type: panda_sim_controllers/PandaGravityController
+    joints:
+      panda_joint1_controller:
+        joint: panda_joint1
+      panda_joint2_controller:
+        joint: panda_joint2
+      panda_joint3_controller:
+        joint: panda_joint3
+      panda_joint4_controller:
+        joint: panda_joint4
+      panda_joint5_controller:
+        joint: panda_joint5
+      panda_joint6_controller:
+        joint: panda_joint6
+      panda_joint7_controller:
+        joint: panda_joint7
+

panda_robot_server/config/panda_sim_controllers_plugins.xml

@@ -0,0 +1,77 @@
+<library path="lib/libpanda_sim_controllers">
+
+    <class name="panda_sim_controllers/PandaGripperController"
+	 type="panda_sim_controllers::PandaGripperController"
+	 base_class_type="controller_interface::ControllerBase">
+        <description>
+           The PandaGripperController tracks position commands for the electric grippers. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/PandaPositionController"
+           type="panda_sim_controllers::PandaPositionController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The PandaPositionController tracks position commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/PandaVelocityController"
+           type="panda_sim_controllers::PandaVelocityController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The PandaPositionController tracks velocity commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/PandaEffortController"
+           type="panda_sim_controllers::PandaEffortController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The PandaPositionController tracks effort commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/PandaGravityController"
+           type="panda_sim_controllers::PandaGravityController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The PandaGravityController adds an effort component to compensate for gravity. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+
+    <class name="panda_sim_controllers/JointPositionController"
+           type="panda_effort_controllers::JointPositionController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointPositionController tracks position commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/JointVelocityController"
+           type="panda_effort_controllers::JointVelocityController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointVelocityController tracks velocity commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/JointEffortController"
+           type="panda_effort_controllers::JointEffortController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="panda_sim_controllers/PandaJointTrajectoryController"
+           type="panda_sim_controllers::PandaJointTrajectoryController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+
+</library>

panda_robot_server/launch/inc/load_panda_sim.launch

@@ -53,7 +53,7 @@
     <arg name="use_sim_time" value="$(arg use_sim_time)"/>
     <arg name="headless" value="$(arg headless)"/>
   </include>
-
+  <!-- <arg name="world_name" value="$(find panda_gazebo)/worlds/panda.world"/> -->
   <!-- Spawn urdf of the robot -->
   <node name="robot_description" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda $(arg initial_joint_states)"/>
 
@@ -84,4 +84,4 @@
 
   </group>
 
-</launch>
+</launch>

panda_robot_server/launch/panda_robot_server.launch

@@ -10,6 +10,7 @@
     <arg name="action_cycle_rate" default="25" doc="Rate at which new commands are published to the robot controller"/>
     <arg name="reference_frame" default="base" doc="Reference frame with respect to which target and ee pose are given"/>
     <arg name="rs_mode"  default="only_robot" doc="Defines the working mode of the Robot Server, this influences the RS state content"/>
+    <arg name="action_mode"  default="delta_pos" doc="Defines the action mode of the Robot Server, this influences the command sent to the robot"/>
 
     <!-- Scene Objects -->
     <arg name="objects_controller" default="false"  doc="Start objects controller node"/>
@@ -87,6 +88,7 @@
         <param name="action_cycle_rate" value="$(arg action_cycle_rate)"></param>
         <param name="reference_frame" value="$(arg reference_frame)"></param>
         <param name="rs_mode" value="$(arg rs_mode)"></param>
+        <param name="action_mode" value="$(arg action_mode)"></param>
     </node>
 
     <!--Launch objects controller -->

panda_robot_server/launch/real_panda_robot_server.launch

@@ -0,0 +1,98 @@
+<?xml version="1.0" ?>
+<launch>
+    <arg name="real_robot" default="false" doc="true: Real Robot, false: Simulated Robot"/>
+    <arg name="gui" default="false"/>
+    <arg name="rviz_gui" default="true"/>
+    <arg name="gazebo_gui" default="false"/>
+    <arg name="world_name" default="empty.world"/>
+    <arg name="max_velocity_scale_factor" default="1.0" doc="Scale factor multiplied by maximum joint velocity value"/>
+    <arg name="server_port" default="50051"/>
+    <arg name="action_cycle_rate" default="25" doc="Rate at which new commands are published to the robot controller"/>
+    <arg name="reference_frame" default="base" doc="Reference frame with respect to which target and ee pose are given"/>
+    <arg name="rs_mode"  default="only_robot" doc="Defines the working mode of the Robot Server, this influences the RS state content"/>
+    <arg name="action_mode"  default="delta_pos" doc="Defines the action mode of the Robot Server, this influences the command sent to the robot"/>
+
+    <!-- Scene Objects -->
+    <arg name="objects_controller" default="false"  doc="Start objects controller node"/>
+    <arg name="n_objects" default="0.0"  doc="Number of objects in the scene"/>
+    <arg name="object_trajectory_file_name" default="no_file" doc="Object trajectory file name"/>
+    <arg name="object_0_model_name" default="" doc="Gazebo model name of the target object"/>
+    <arg name="object_0_frame" default="" doc="TF Frame name of target"/>
+    <arg name="object_1_model_name" default="" doc="Gazebo model name of object 1"/>
+    <arg name="object_1_frame" default="" doc="TF Frame name of object 1"/>
+
+    <!-- RViz Configuration -->
+    <arg name="rviz_config_path" default="$(find panda_robot_server)/rviz"/>
+    <arg name="rviz_config_file" default="panda_sim_rl.rviz" doc="RViz configuration file"/>
+    <arg name="rviz_config" value="$(arg rviz_config_path)/$(arg rviz_config_file)"/>
+
+    <!-- Robot Base position -->
+    <!-- NOTE: Assuming the UR5 positions ATM -->
+    <arg name="x"       default="0.0"   doc="base_link coordinate with respect to the world frame"/>
+    <arg name="y"       default="0.0"   doc="base_link coordinate with respect to the world frame"/>
+    <arg name="z"       default="0.1"   doc="base_link coordinate with respect to the world frame"/>
+    <arg name="roll"    default="0.0"   doc="base_link coordinate with respect to the world frame"/>
+    <arg name="pitch"   default="0.0"   doc="base_link coordinate with respect to the world frame"/>
+    <arg name="yaw"     default="0.0"   doc="base_link coordinate with respect to the world frame"/>
+
+    <!-- Camera 1 activate and position -->
+    <!-- <arg name="camera1_gazebo" default="False" doc="use camera1 gazebo simulated sensor"/>
+    <arg name="camera1_link_x" default="0.0" doc="camera1_link coordinate with respect to the world frame"/>
+    <arg name="camera1_link_y" default="0.0" doc="camera1_link coordinate with respect to the world frame"/>
+    <arg name="camera1_link_z" default="0.1" doc="camera1_link coordinate with respect to the world frame"/>
+    <arg name="camera1_link_roll" default="0.0" doc="camera1_link coordinate with respect to the world frame"/>
+    <arg name="camera1_link_pitch" default="0.0" doc="camera1_link coordinate with respect to the world frame"/>
+    <arg name="camera1_link_yaw" default="0.0" doc="camera1_link coordinate with respect to the world frame"/> -->
+
+    <!-- Robot description and related parameter files -->
+    <!-- <arg name="robot_description_file" default="$(dirname)/inc/load_panda_sim.launch" doc="Launch file which populates the 'robot_description' parameter."/> -->
+
+    <!-- Controller configurations -->
+
+
+    <!-- Start the 'driver' (ie: Gazebo in this case) -->
+    <!--<include file="$(dirname)/inc/load_panda_sim.launch">
+        <arg name="gui" value="$(eval arg('gui') and arg('gazebo_gui'))"/>
+        <arg name="paused" value="false"/>
+        <arg name="gazebo_world" value="$(arg world_name)"/>
+    </include>-->
+
+    <!-- Launch Command Handler -->
+    <!-- <node name="joint_trajectory_command_handler" pkg="panda_robot_server" type="joint_trajectory_command_handler.py" respawn="False" output="screen">
+        <param name="real_robot" value="false"/>
+        <param name="action_cycle_rate" value="$(arg action_cycle_rate)"></param>
+    </node> -->
+
+    <!-- Launch RViz -->
+    <!-- <node if="$(eval arg('gui') and arg('rviz_gui'))" name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(arg rviz_config)" output="screen">
+    </node> -->
+
+    <!-- Global Parameters -->
+    <param name="real_robot" value="$(arg real_robot)"></param>
+    <param name="reference_frame" value="$(arg reference_frame)"></param>
+
+    <!-- Scene Objects Parameters -->
+    <param name="objects_controller" value="$(arg objects_controller)"></param>
+    <param name="n_objects" value="$(arg n_objects)"></param>
+    <param name="object_trajectory_file_name" value="$(arg object_trajectory_file_name)"></param>
+    <param name="object_0_model_name" value="$(arg object_0_model_name)"></param>
+    <param name="object_0_frame" value="$(arg object_0_frame)"></param>
+    <param name="object_1_model_name" value="$(arg object_1_model_name)"></param>
+    <param name="object_1_frame" value="$(arg object_1_frame)"></param>
+
+    <!--Launch gRPC server -->
+    <node name="robot_server" pkg="panda_robot_server" type="robot_server.py" respawn="false" output="screen">
+        <param name="server_port" value="$(arg server_port)"></param>
+        <param name="real_robot" value="true"></param>
+        <param name="max_velocity_scale_factor" value="$(arg max_velocity_scale_factor)"></param>
+        <param name="action_cycle_rate" value="$(arg action_cycle_rate)"></param>
+        <param name="reference_frame" value="$(arg reference_frame)"></param>
+        <param name="rs_mode" value="$(arg rs_mode)"></param>
+        <param name="action_mode" value="$(arg action_mode)"></param>
+    </node>
+
+    <!--Launch objects controller -->
+    <node if="$(arg objects_controller)" name="objects_controller" pkg="simulation_objects" type="objects_controller.py" respawn="false" output="screen" ></node>
+
+
+</launch>

panda_robot_server/scripts/robot_server.py

@@ -28,15 +28,15 @@ def SetState(self, request, context):
 
     def SendAction(self, request, context):
         try:
-            executed_action = self.rosbridge.publish_env_arm_cmd(request.action)
+            executed_action = self.rosbridge.send_action(request.action)
             return robot_server_pb2.Success(success=1)
         except:
             rospy.logerr('Failed to send action', exc_info=True)
             return robot_server_pb2.Success(success=0)
 
     def SendActionGetState(self, request, context):
         try:
-            executed_action = self.rosbridge.publish_env_arm_cmd(request.action)
+            executed_action = self.rosbridge.send_action(request.action)
             return self.rosbridge.get_state()
         except:
             rospy.logerr('Failed to send action and get state', exc_info=True)