Added joint velocity and cartesian velocity robot control.

franka-interface-common/include/franka-interface-common/definitions.h

@@ -22,10 +22,12 @@ typedef SensorBufferType* SensorBufferTypePtr;
 // Enum for Skill Types
 enum class SkillType : uint8_t {
     CartesianPoseSkill,
+    CartesianVelocitySkill,
     ForceTorqueSkill,
     GripperSkill,
     ImpedanceControlSkill,
     JointPositionSkill,
+    JointVelocitySkill,
 };
 
 // Enum for Meta Skill Types
@@ -36,19 +38,23 @@ enum class MetaSkillType : uint8_t {
 
 // Enum for Trajectory Generator Types
 enum class TrajectoryGeneratorType : uint8_t {
+    CartesianVelocityTrajectoryGenerator,
     CubicHermiteSplineJointTrajectoryGenerator,
     CubicHermiteSplinePoseTrajectoryGenerator,
     GoalPoseDmpTrajectoryGenerator,
     GripperTrajectoryGenerator,
     ImpulseTrajectoryGenerator,
     JointDmpTrajectoryGenerator,
+    JointVelocityTrajectoryGenerator,
     LinearForcePositionTrajectoryGenerator,
     LinearJointTrajectoryGenerator,
     LinearPoseTrajectoryGenerator,
     MinJerkJointTrajectoryGenerator,
     MinJerkPoseTrajectoryGenerator,
+    PassThroughCartesianVelocityTrajectoryGenerator,
     PassThroughForcePositionTrajectoryGenerator,
     PassThroughJointTrajectoryGenerator,
+    PassThroughJointVelocityTrajectoryGenerator,
     PassThroughPoseTrajectoryGenerator,
     PoseDmpTrajectoryGenerator,
     QuaternionPoseDmpTrajectoryGenerator,

franka-interface/CMakeLists.txt

@@ -40,31 +40,37 @@ add_library(franka-interface SHARED
   src/skills/base_meta_skill.cpp
   src/skills/base_skill.cpp
   src/skills/cartesian_pose_skill.cpp
+  src/skills/cartesian_velocity_skill.cpp
   src/skills/force_torque_skill.cpp
   src/skills/gripper_skill.cpp
   src/skills/impedance_control_skill.cpp
   src/skills/joint_position_continuous_skill.cpp
   src/skills/joint_position_skill.cpp
+  src/skills/joint_velocity_skill.cpp
   src/termination_handler/contact_termination_handler.cpp
   src/termination_handler/final_joint_termination_handler.cpp
   src/termination_handler/final_pose_termination_handler.cpp
   src/termination_handler/noop_termination_handler.cpp
   src/termination_handler/termination_handler.cpp
   src/termination_handler/time_termination_handler.cpp
+  src/trajectory_generator/cartesian_velocity_trajectory_generator.cpp
   src/trajectory_generator/cubic_hermite_spline_joint_trajectory_generator.cpp
   src/trajectory_generator/cubic_hermite_spline_pose_trajectory_generator.cpp
   src/trajectory_generator/goal_pose_dmp_trajectory_generator.cpp
   src/trajectory_generator/gripper_trajectory_generator.cpp
   src/trajectory_generator/impulse_trajectory_generator.cpp
   src/trajectory_generator/joint_dmp_trajectory_generator.cpp
   src/trajectory_generator/joint_trajectory_generator.cpp
+  src/trajectory_generator/joint_velocity_trajectory_generator.cpp
   src/trajectory_generator/linear_force_position_trajectory_generator.cpp
   src/trajectory_generator/linear_joint_trajectory_generator.cpp
   src/trajectory_generator/linear_pose_trajectory_generator.cpp
   src/trajectory_generator/min_jerk_joint_trajectory_generator.cpp
   src/trajectory_generator/min_jerk_pose_trajectory_generator.cpp
+  src/trajectory_generator/pass_through_cartesian_velocity_trajectory_generator.cpp
   src/trajectory_generator/pass_through_force_position_trajectory_generator.cpp
   src/trajectory_generator/pass_through_joint_trajectory_generator.cpp
+  src/trajectory_generator/pass_through_joint_velocity_trajectory_generator.cpp
   src/trajectory_generator/pass_through_pose_trajectory_generator.cpp
   src/trajectory_generator/pose_dmp_trajectory_generator.cpp
   src/trajectory_generator/pose_trajectory_generator.cpp

franka-interface/include/franka-interface/skills/cartesian_velocity_skill.h

@@ -0,0 +1,21 @@
+#ifndef FRANKA_INTERFACE_SKILLS_CARTESIAN_VELOCITY_SKILL_H_
+#define FRANKA_INTERFACE_SKILLS_CARTESIAN_VELOCITY_SKILL_H_
+
+#include "franka-interface/skills/base_skill.h"
+
+class CartesianVelocitySkill : public BaseSkill {
+ public:
+  CartesianVelocitySkill(int skill_idx, int meta_skill_idx, std::string description) : 
+                              BaseSkill(skill_idx, meta_skill_idx, description)
+  {};
+
+  void execute_skill_on_franka(run_loop* run_loop,
+                               FrankaRobot* robot,
+                               FrankaGripper* gripper,
+                               RobotStateData* robot_state_data) override;
+
+ private:
+  bool return_status_{false};
+};
+
+#endif  // FRANKA_INTERFACE_SKILLS_CARTESIAN_VELOCITY_SKILL_H_

franka-interface/include/franka-interface/skills/joint_velocity_skill.h

@@ -0,0 +1,21 @@
+#ifndef FRANKA_INTERFACE_SKILLS_JOINT_VELOCITY_SKILL_H_
+#define FRANKA_INTERFACE_SKILLS_JOINT_VELOCITY_SKILL_H_
+
+#include "franka-interface/skills/base_skill.h"
+
+class JointVelocitySkill : public BaseSkill {
+ public:
+  JointVelocitySkill(int skill_idx, int meta_skill_idx, std::string description) : 
+                              BaseSkill(skill_idx, meta_skill_idx, description)
+  {};
+
+  void execute_skill_on_franka(run_loop* run_loop,
+                               FrankaRobot* robot,
+                               FrankaGripper* gripper,
+                               RobotStateData* robot_state_data) override;
+
+ private:
+  bool return_status_{false};
+};
+
+#endif  // FRANKA_INTERFACE_SKILLS_JOINT_VELOCITY_SKILL_H_

franka-interface/include/franka-interface/trajectory_generator/cartesian_velocity_trajectory_generator.h

@@ -0,0 +1,36 @@
+#ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_
+#define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_
+
+#include <array>
+
+#include "franka-interface/trajectory_generator/trajectory_generator.h"
+
+class CartesianVelocityTrajectoryGenerator : public TrajectoryGenerator {
+ public:
+  using TrajectoryGenerator::TrajectoryGenerator;
+
+  void parse_parameters() override;
+
+  void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::CartesianVelocitySkill) override;
+
+  /**
+   * Initialize initial and desired Cartesian velocities from robot state
+   */
+  void initialize_initial_cartesian_velocities(const franka::RobotState &robot_state, SkillType skill_type);
+
+  /**
+   * Returns the desired Cartesian velocities. This method is called at every time step of the control loop to
+   * find the Cartesian velocities to send at the next step.
+   */
+  const std::array<double, 6>& get_desired_cartesian_velocities() const;
+
+ protected:
+  CartesianVelocityTrajectoryGeneratorMessage cartesian_velocity_trajectory_params_;
+
+  std::array<double, 6> initial_cartesian_velocities_{};
+  std::array<double, 6> cartesian_accelerations_{};
+  std::array<double, 6> desired_cartesian_velocities_{};
+  std::array<double, 6> goal_cartesian_velocities_{};
+};
+
+#endif	// FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_

franka-interface/include/franka-interface/trajectory_generator/joint_velocity_trajectory_generator.h

@@ -0,0 +1,36 @@
+#ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_
+#define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_
+
+#include <array>
+
+#include "franka-interface/trajectory_generator/trajectory_generator.h"
+
+class JointVelocityTrajectoryGenerator : public TrajectoryGenerator {
+ public:
+  using TrajectoryGenerator::TrajectoryGenerator;
+
+  void parse_parameters() override;
+
+  void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::JointVelocitySkill) override;
+
+  /**
+   * Initialize initial and desired joint velocities from robot state
+   */
+  void initialize_initial_joint_velocities(const franka::RobotState &robot_state, SkillType skill_type);
+
+  /**
+   * Returns the desired joint velocities. This method is called at every time step of the control loop to
+   * find the joint velocities to send at the next step.
+   */
+  const std::array<double, 7>& get_desired_joint_velocities() const;
+
+ protected:
+  JointVelocityTrajectoryGeneratorMessage joint_velocity_trajectory_params_;
+
+  std::array<double, 7> initial_joint_velocities_{};
+  std::array<double, 7> joint_accelerations_{};
+  std::array<double, 7> desired_joint_velocities_{};
+  std::array<double, 7> goal_joint_velocities_{};
+};
+
+#endif	// FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_

franka-interface/include/franka-interface/trajectory_generator/pass_through_cartesian_velocity_trajectory_generator.h

@@ -0,0 +1,19 @@
+#ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_
+#define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_
+
+#include "franka-interface/trajectory_generator/cartesian_velocity_trajectory_generator.h"
+
+class PassThroughCartesianVelocityTrajectoryGenerator : public CartesianVelocityTrajectoryGenerator {
+ public:
+  using CartesianVelocityTrajectoryGenerator::CartesianVelocityTrajectoryGenerator;
+
+  void get_next_step(const franka::RobotState &robot_state) {};
+
+  void parse_sensor_data(const franka::RobotState &robot_state) override;
+
+ private:
+  CartesianVelocitySensorMessage cartesian_velocity_sensor_msg_;
+
+};
+
+#endif	// FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_

franka-interface/include/franka-interface/trajectory_generator/pass_through_joint_velocity_trajectory_generator.h

@@ -0,0 +1,19 @@
+#ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_
+#define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_
+
+#include "franka-interface/trajectory_generator/joint_velocity_trajectory_generator.h"
+
+class PassThroughJointVelocityTrajectoryGenerator : public JointVelocityTrajectoryGenerator {
+ public:
+  using JointVelocityTrajectoryGenerator::JointVelocityTrajectoryGenerator;
+
+  void get_next_step(const franka::RobotState &robot_state) {};
+
+  void parse_sensor_data(const franka::RobotState &robot_state) override;
+
+ private:
+  JointVelocitySensorMessage joint_velocity_sensor_msg_;
+
+};
+
+#endif	// FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_

franka-interface/proto/sensor_msg.proto

@@ -31,6 +31,13 @@ message PosePositionVelocitySensorMessage {
   repeated double pose_velocities = 6;
 }
 
+message JointVelocitySensorMessage {
+  required int32 id = 1;
+  required double timestamp = 2;
+
+  repeated double joint_velocities = 3;
+}
+
 message JointPositionSensorMessage {
   required int32 id = 1;
   required double timestamp = 2;
@@ -46,6 +53,13 @@ message PosePositionSensorMessage {
   repeated double quaternion = 4;
 }
 
+message CartesianVelocitySensorMessage {
+  required int32 id = 1;
+  required double timestamp = 2;
+
+  repeated double cartesian_velocities = 3;
+}
+
 message ShouldTerminateSensorMessage {
   required double timestamp = 1;
   required bool should_terminate = 2;

franka-interface/proto/trajectory_generator_params_msg.proto

@@ -23,6 +23,20 @@ message JointTrajectoryGeneratorMessage {
   repeated double joints = 2;
 }
 
+message JointVelocityTrajectoryGeneratorMessage {
+  required double run_time = 1;
+
+  repeated double joint_velocities = 2;
+  repeated double joint_accelerations = 3;
+}
+
+message CartesianVelocityTrajectoryGeneratorMessage {
+  required double run_time = 1;
+
+  repeated double cartesian_velocities = 2;
+  repeated double cartesian_accelerations = 3;
+}
+
 message PoseTrajectoryGeneratorMessage {
   required double run_time = 1;
 

franka-interface/src/run_loop.cpp

@@ -21,11 +21,13 @@
 #include "franka-interface/skills/base_meta_skill.h"
 #include "franka-interface/skills/base_skill.h"
 #include "franka-interface/skills/cartesian_pose_skill.h"
+#include "franka-interface/skills/cartesian_velocity_skill.h"
 #include "franka-interface/skills/force_torque_skill.h"
 #include "franka-interface/skills/gripper_skill.h"
 #include "franka-interface/skills/impedance_control_skill.h"
 #include "franka-interface/skills/joint_position_continuous_skill.h"
 #include "franka-interface/skills/joint_position_skill.h"
+#include "franka-interface/skills/joint_velocity_skill.h"
 #include "franka-interface/utils/logger_utils.h"
 
 std::atomic<bool> run_loop::run_loop_ok_{false};
@@ -239,6 +241,10 @@ void run_loop::update_process_info() {
               skill_type_name = "CartesianPoseSkill";
               new_skill = new CartesianPoseSkill(new_skill_id, new_meta_skill_id, new_skill_description);
               break;
+            case SkillType::CartesianVelocitySkill:
+              skill_type_name = "CartesianVelocitySkill";
+              new_skill = new CartesianVelocitySkill(new_skill_id, new_meta_skill_id, new_skill_description);
+              break;
             case SkillType::ForceTorqueSkill:
               skill_type_name = "ForceTorqueSkill";
               new_skill = new ForceTorqueSkill(new_skill_id, new_meta_skill_id, new_skill_description);
@@ -255,6 +261,10 @@ void run_loop::update_process_info() {
               skill_type_name = "JointPositionSkill";
               new_skill = new JointPositionSkill(new_skill_id, new_meta_skill_id, new_skill_description);
               break;
+            case SkillType::JointVelocitySkill:
+              skill_type_name = "JointVelocitySkill";
+              new_skill = new JointVelocitySkill(new_skill_id, new_meta_skill_id, new_skill_description);
+              break;
             default:
               std::cout << "Incorrect skill type: " << 
               static_cast<std::underlying_type<SkillType>::type>(new_skill_type) <<