Adds position and velocity interfaces to FrankaHWSim

This commit adds the 'hardware_interface/PositionJointInterface' and
'hardware_interface/VelocityJointInterface' interfaces to the
FrankaHWSim module. This was done to make the simulation closer match
the real robot.

Removes formatting changes

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -29,6 +29,8 @@ namespace franka_gazebo {
  * ### transmission_interface/SimpleTransmission
  * - hardware_interface/JointStateInterface
  * - hardware_interface/EffortJointInterface
+ * - hardware_interface/PositionJointInterface
+ * - hardware_interface/VelocityJointInterface
  *
  * ### franka_hw/FrankaStateInterface
  * ### franka_hw/FrankaModelInterface
@@ -96,6 +98,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
 
  hardware_interface::JointStateInterface jsi_;
  hardware_interface::EffortJointInterface eji_;
+ hardware_interface::PositionJointInterface pji_;
+ hardware_interface::VelocityJointInterface vji_;
  franka_hw::FrankaStateInterface fsi_;
  franka_hw::FrankaModelInterface fmi_;
 
@@ -112,6 +116,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
 
  void initJointStateHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
  void initEffortCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
+ void initPositionCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
+ void initVelocityCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
  void initFrankaStateHandle(const std::string& robot,
  const urdf::Model& urdf,
  const transmission_interface::TransmissionInfo& transmission);

franka_gazebo/launch/panda.launch

@@ -12,6 +12,7 @@
  <arg name="arm_id" default="panda" doc="Name of the panda robot to spawn" />
  <arg name="use_gripper" default="true" doc="Should a franka hand be mounted on the flange?" />
  <arg name="controller" default=" " doc="Which example controller should be started? (One of {cartesian_impedance,model,force}_example_controller)" />
+ <arg name="transmission" default="hardware_interface/EffortJointInterface" doc="Which transmission should be used for the panda joints? (One of hardware_interface/{EffortJointInterface,PositionJointInterface,VelocityJointInterface})" />
  <arg name="x" default="0" doc="How far forward to place the base of the robot in [m]?" />
  <arg name="y" default="0" doc="How far leftwards to place the base of the robot in [m]?" />
  <arg name="z" default="0" doc="How far upwards to place the base of the robot in [m]?" />
@@ -42,6 +43,7 @@
  gazebo:=true
  hand:=$(arg use_gripper)
  arm_id:=$(arg arm_id)
+ transmission:=$(arg transmission)
  xyz:='$(arg x) $(arg y) $(arg z)'
  rpy:='$(arg roll) $(arg pitch) $(arg yaw)'">
  </param>

franka_gazebo/src/franka_hw_sim.cpp

@@ -107,6 +107,12 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
  if (k_interface == "hardware_interface/EffortJointInterface") {
  initEffortCommandHandle(joint);
  continue;
+ } else if (k_interface == "hardware_interface/PositionJointInterface") {
+ initPositionCommandHandle(joint);
+ continue;
+ } else if (k_interface == "hardware_interface/VelocityJointInterface") {
+ initVelocityCommandHandle(joint);
+ continue;
  }
  }
 
@@ -142,6 +148,8 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
 
  // After all handles have been assigned to interfaces, register them
  registerInterface(&this->eji_);
+ registerInterface(&this->pji_);
+ registerInterface(&this->vji_);
  registerInterface(&this->jsi_);
  registerInterface(&this->fsi_);
  registerInterface(&this->fmi_);
@@ -162,6 +170,16 @@ void FrankaHWSim::initEffortCommandHandle(const std::shared_ptr<franka_gazebo::J
  hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command));
 }
 
+void FrankaHWSim::initPositionCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint) {
+ this->pji_.registerHandle(
+ hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command));
+}
+
+void FrankaHWSim::initVelocityCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint) {
+ this->vji_.registerHandle(
+ hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command));
+}
+
 void FrankaHWSim::initFrankaStateHandle(
  const std::string& robot,
  const urdf::Model& urdf,