Joint Trajectory Controller Improvements

kinova_driver/include/kinova/KinovaTypes.h

@@ -468,6 +468,31 @@ struct AngularInfo
 		Actuator6 = 0.0f;
 		Actuator7 = 0.0f;
 	}
+
+	float& operator [](const int idx){
+		switch (idx)
+		{
+			case 0: return Actuator1;
+			case 1: return Actuator2;
+			case 2: return Actuator3;
+			case 3: return Actuator4;
+			case 4: return Actuator5;
+			case 5: return Actuator6;
+			case 6: return Actuator7;
+		}
+	}
+    float operator [](const int idx) const{
+		switch (idx)
+		{
+			case 0: return Actuator1;
+			case 1: return Actuator2;
+			case 2: return Actuator3;
+			case 3: return Actuator4;
+			case 4: return Actuator5;
+			case 5: return Actuator6;
+			case 6: return Actuator7;
+		}
+	}
 };
 
 /** @brief This data structure holds values in an cartesian control context.

kinova_driver/include/kinova_driver/kinova_arm.h

@@ -18,6 +18,8 @@
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
 #include <sensor_msgs/JointState.h>
+#include <sensor_msgs/Joy.h>
+#include <std_srvs/Trigger.h>
 
 #include <kinova_msgs/Stop.h>
 #include <kinova_msgs/Start.h>
@@ -78,6 +80,8 @@ class KinovaArm
     // Service callbacks -----------------------------------------------------------
     bool stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs::Stop::Response &res);
     bool startServiceCallback(kinova_msgs::Start::Request &req, kinova_msgs::Start::Response &res);
+    bool cartesian_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
+    bool angular_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
     bool homeArmServiceCallback(kinova_msgs::HomeArm::Request &req, kinova_msgs::HomeArm::Response &res);
     bool ActivateNullSpaceModeCallback(kinova_msgs::SetNullSpaceModeState::Request &req,
                                        kinova_msgs::SetNullSpaceModeState::Response &res);
@@ -116,6 +120,7 @@ class KinovaArm
     void publishToolPosition(void);
     void publishToolWrench(void);
     void publishFingerPosition(void);
+    void publishJoystickCommand(void);
 
     tf::TransformListener tf_listener_;
     ros::NodeHandle node_handle_;
@@ -135,12 +140,15 @@ class KinovaArm
     ros::Publisher tool_wrench_publisher_;
     ros::Publisher finger_position_publisher_;
     ros::Publisher joint_state_publisher_;
+    ros::Publisher joystick_command_publisher_;
 
     ros::Publisher joint_command_publisher_;
     ros::Publisher cartesian_command_publisher_;
 
     ros::ServiceServer stop_service_;
     ros::ServiceServer start_service_;
+    ros::ServiceServer cartesian_control_service_;
+    ros::ServiceServer angular_control_service_;
     ros::ServiceServer homing_service_;
     ros::ServiceServer start_null_space_service_;
     ros::ServiceServer add_trajectory_;

kinova_driver/include/kinova_driver/kinova_comm.h

@@ -88,6 +88,7 @@ class KinovaComm
     void initFingers(void);
     void setEndEffectorOffset(unsigned int status, float x, float y, float z);
     void getEndEffectorOffset(unsigned int &status, float &x, float &y, float &z);
+    void getJoystickCommand(JoystickCommand &joystick_command);
 
     // %EndTag(general functions)
 

kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h

@@ -1,7 +1,6 @@
 #ifndef KINOVA_JOINT_TRAJECTORY_CONTROLLER_H
 #define KINOVA_JOINT_TRAJECTORY_CONTROLLER_H
 
-
 #include <ros/ros.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
 #include <sensor_msgs/JointState.h>
@@ -26,64 +25,45 @@ class JointTrajectoryController
 private:
     ros::NodeHandle nh_;
 
-    ros::Subscriber sub_command_;
-    ros::Publisher pub_joint_feedback_;
-    ros::Publisher pub_joint_velocity_;
+    ros::Subscriber sub_command_; // command subscriber listening to 'trajectory_controller/command'
+    ros::Publisher pub_joint_feedback_; // feedback publisher sending to 'trajectory_controller/state'
+    ros::Publisher pub_joint_velocity_; // velocity publisher sending to 'in/joint_velocity'
 
-    ros::Time previous_pub_;
-    ros::Time time_pub_joint_vel_;
+    ros::Time time_pub_joint_vel_; // time of timer thread start to publish joint velocity command
 
-    ros::Timer timer_pub_joint_vel_;
+    ros::Timer timer_pub_joint_vel_; // timer to publish joint velocities
     boost::mutex terminate_thread_mutex_;
     boost::thread* thread_update_state_;
     bool terminate_thread_;
 
-    std_msgs::Duration time_from_start_;
-    sensor_msgs::JointState current_joint_state_;
-
-    KinovaComm kinova_comm_;
-    TrajectoryPoint kinova_traj_point_;
+    KinovaComm kinova_comm_; // Communication pipeline for the arm
 
-//    trajectory_msgs::JointTrajectory joint_traj_;
-//    trajectory_msgs::JointTrajectoryPoint joint_traj_point_;
-    std::string traj_frame_id_;
-    std::vector<trajectory_msgs::JointTrajectoryPoint> traj_command_points_;
-    control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_;
+    std::string traj_frame_id_; // origin of trajectory transforms
+    std::vector<trajectory_msgs::JointTrajectoryPoint> traj_command_points_; // trajectory points from most recent trajectory
+    control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_; // trajectory feedback message
 
     // stores the command to send to robot, in Kinova type (KinovaAngles)
-    std::vector<KinovaAngles> kinova_angle_command_;
-
-    uint number_joint_;
-    int traj_command_points_index_;
-    std::vector<std::string> joint_names_;
-    std::string prefix_;
-
-    struct Segment
-    {
-        double start_time;
-        double duration;
-        std::vector<double> positions;
-        std::vector<double> velocities;
-    };
+    std::vector<KinovaAngles> kinova_angle_command_; // intermediate command storage
 
+    uint number_joint_; // number of joints of the robot
+    int traj_command_points_index_; // current index in traj_command_points_, defined by time
+    std::vector<std::string> joint_names_; // names of the joints
+    std::string prefix_; // robot name prefix
+    const static int num_possible_joints = 7; // number of possible joints supported by the system
+    float current_velocity_command[num_possible_joints]; // storage array to keep calculated velocity commands
+    double remaining_motion_time[num_possible_joints]; // time of motion remaining for each joint during the last command
 
     // call back function when receive a trajectory command
     void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &traj_msg);
 
     // reflash the robot state and publish the joint state: either by timer or thread
     void update_state(); // by thread
 
+    // regularly publish the velocity commands of the trajectory 
     void pub_joint_vel(const ros::TimerEvent&); // by timer
-    int test;
 
 };
 
-
-
-
-
-
 }
 
-
 #endif // KINOVA_JOINT_TRAJECTORY_CONTROLLER_H

kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp

@@ -236,21 +236,23 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll
     {
         // Checks that we have ended inside the goal constraints
         bool inside_goal_constraints = true;
-        for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i)
+        bool inside_velocity_constraints = true;
+        for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints && inside_velocity_constraints; ++i)
         {
             // computing error from goal pose
             double abs_error = fabs(msg->actual.positions[i] - current_traj_.points[last].positions[i]);
+            abs_error = abs_error - 2*M_PI*std::floor((abs_error+M_PI)/(2*M_PI)); // fix rotation to [-PI,PI)
             double goal_constraint = goal_constraints_[msg->joint_names[i]];
             if (goal_constraint >= 0 && abs_error > goal_constraint)
                 inside_goal_constraints = false;
             // It's important to be stopped if that's desired.
             if ( !(msg->desired.velocities.empty()) && (fabs(msg->desired.velocities[i]) < 1e-6) )
             {
                 if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
-                    inside_goal_constraints = false;
+                    inside_velocity_constraints = false;
             }
         }
-        if (inside_goal_constraints)
+        if (inside_goal_constraints && inside_velocity_constraints)
         {
             active_goal_.setSucceeded();
             has_active_goal_ = false;
@@ -262,7 +264,17 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll
         }
         else
         {
-            ROS_WARN("Aborting because we wound up outside the goal constraints");
+            ROS_WARN("Aborting because we wound up outside the %s constraints", inside_velocity_constraints?"goal":"velocity");
+            for(int j = 0; j<7; j++){
+                double abs_error; 
+                if(!inside_goal_constraints){
+                    abs_error = fabs(msg->actual.positions[j] - current_traj_.points[last].positions[j]); 
+                    abs_error = abs_error - 2*M_PI*std::floor((abs_error+M_PI)/(2*M_PI)); // fix rotation to [-PI,PI)
+                } else {
+                    abs_error = fabs(msg->actual.velocities[j] - stopped_velocity_tolerance_);
+                }
+                ROS_WARN_STREAM("Error " << j << ": " << abs_error);
+            }
             active_goal_.setAborted();
             has_active_goal_ = false;
         }

kinova_driver/src/kinova_arm.cpp

@@ -165,6 +165,8 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s
     /* Set up Services */
     stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this);
     start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this);
+    cartesian_control_service_ = node_handle_.advertiseService("in/cartesian_control", &KinovaArm::cartesian_controlServiceCallback, this);
+    angular_control_service_ = node_handle_.advertiseService("in/angular_control", &KinovaArm::angular_controlServiceCallback, this);
     homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this);
     add_trajectory_ = node_handle_.advertiseService("in/add_pose_to_Cartesian_trajectory",
                       &KinovaArm::addCartesianPoseToTrajectory, this);
@@ -202,6 +204,8 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s
             ("out/tool_wrench", 2);
     finger_position_publisher_ = node_handle_.advertise<kinova_msgs::FingerPosition>
             ("out/finger_position", 2);
+    joystick_command_publisher_ = node_handle_.advertise<sensor_msgs::Joy>
+            ("out/joystick_command", 2);
 
     // Publish last command for relative motion (read current position cause arm drop)
     joint_command_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>("out/joint_command", 2);
@@ -370,6 +374,29 @@ bool KinovaArm::stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs
     return true;
 }
 
+/*
+ * Handler for service to change to cartesian control mode.
+ */
+bool KinovaArm::cartesian_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
+{
+    kinova_comm_.setCartesianControl();
+    res.message = "Cartesian Control activated";
+    res.success = true;
+    ROS_DEBUG("Cartesian Control requested");
+    return true;
+}
+
+/*
+ * Handler for service to change to angular control mode.
+ */
+bool KinovaArm::angular_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
+{
+    kinova_comm_.setAngularControl();
+    res.message  = "Angular Control activated";
+    res.success = true;
+    ROS_DEBUG("Angular Control requested");
+    return true;
+}
 
 /*!
  * \brief Handler for "start" service.
@@ -756,12 +783,41 @@ void KinovaArm::publishFingerPosition(void)
     finger_position_publisher_.publish(fingers.constructFingersMsg());
 }
 
+/**
+ * \brief Publishes the current command of the joystick 
+ */
+void KinovaArm::publishJoystickCommand(void)
+{
+    JoystickCommand command;
+    kinova_comm_.getJoystickCommand(command);
+
+    sensor_msgs::Joy msg;
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = "kinova_joystick";
+    msg.axes = {command.InclineLeftRight,
+                command.InclineForwardBackward,
+                command.Rotate,
+                command.MoveLeftRight,
+                command.MoveForwardBackward,
+                command.PushPull
+               };
+    // The ButtonValue array seems to be ill-formed:  
+    // Tests show it consists of 1-byte (char) sized values and is actually offset by 4 bytes
+    const int data_offset_bytes = 4;
+    const char* actual_button_array = (char*) command.ButtonValue + data_offset_bytes;
+    for(int i=0; i<JOYSTICK_BUTTON_COUNT; i++){
+        msg.buttons.push_back(actual_button_array[i]);
+    }    
+    joystick_command_publisher_.publish(msg);
+}
+
 void KinovaArm::statusTimer(const ros::TimerEvent&)
 {
     publishJointAngles();
     publishToolPosition();
     publishToolWrench();
     publishFingerPosition();
+    publishJoystickCommand();
 }
 
 }  // namespace kinova

kinova_driver/src/kinova_comm.cpp

@@ -419,6 +419,21 @@ void KinovaComm::getControlType(int &controlType)
     }
 }
 
+/**
+ * Requests the current joystick command from the robotical arm
+ * Warning: There might be an issue with the ButtonValue Attribute
+ *          Our tests show them to be a byte array with an initial offset of 4 bytes
+ */
+void KinovaComm::getJoystickCommand(JoystickCommand &joystick_command)
+{
+    boost::recursive_mutex::scoped_lock lock(api_mutex_);
+    int result = kinova_api_.getJoystickValue(joystick_command);
+    if (result!=NO_ERROR_KINOVA)
+    {
+        throw KinovaCommException("Could not get joystick command", result);
+    }
+}
+
 
 /**
  * @brief get almost all information of the robotical arm.
@@ -958,7 +973,6 @@ void KinovaComm::setCartesianControl()
         return;
     }
     int result = kinova_api_.setCartesianControl();
-    ROS_WARN("%d", result);
     if (result != NO_ERROR_KINOVA)
     {
         throw KinovaCommException("Could not set Cartesian control", result);