Detection dei salti di giunto funzionante

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -98,6 +98,7 @@ typedef struct {
     robot_state::RobotStatePtr kine_state;
     std::vector<std::string> joint_names;
     std::map<std::string, float> joint_homes;
+    double joint_jump_tresh;
 } robotData;
 
 typedef struct {
@@ -114,4 +115,4 @@ std::map<std::string, double> floatMap2doubleMap(std::map<std::string,float>);
 double e3dist(geometry_msgs::Twist, geometry_msgs::Twist);
 double rad2deg(double ang);
 double deg2rad(double ang);
-bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double);
+bool checkJointJump(moveit_msgs::RobotTrajectory*, double);

src/roboglue_ros/launch/roboglue_ros.launch

@@ -29,6 +29,7 @@
         <param name="move_group_name" type="string" value="manipulator" />
         <param name="loop_rate" type="int" value="100" />
         <param name="msg_buffer" type="int" value="100" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
         <param name="min_nonzero_move" type="double" value="0.005" />
     </group>
 

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -33,6 +33,7 @@
         <param name="move_group_name" type="string" value="manipulator" />
         <param name="loop_rate" type="int" value="100" />
         <param name="msg_buffer" type="int" value="100" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
         <param name="min_nonzero_move" type="double" value="0.005" />
     </group>
 

src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch

@@ -30,6 +30,7 @@
         <param name="robot_model_name" type="string" value="robot_description" />
         <param name="move_group_name" type="string" value="manipulator" />
         <param name="min_nonzero_move" type="double" value="0.005" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
         <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
         <!-- ROS node loop parameters -->
         <param name="loop_rate" type="int" value="100" />

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -33,6 +33,7 @@
             che non si incappi in configurazioni instabili.
 20190708 - Implementazione funzioni di visualizzazione del percorso e pulizia dello schermo prima
             dell'esecuzione del file.
+20191029 - Spostata la documentazione si Git
 */
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
@@ -260,6 +261,7 @@ int main(int argc, char **argv) {
     // load robot model information
     nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
     nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam(param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
 
     /// set console log level ///
     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
@@ -365,17 +367,18 @@ int main(int argc, char **argv) {
                     move_group.setStartStateToCurrentState();
                     move_group.computeCartesianPath(pss,ee_step,jmp_thr,traj, false, &ec);
                     ROS_DEBUG("Trajectory Compute ExitCode: %d",ec.val);
-//                    BUG: il controllo del salto dei giunti crasha il nodo.
+                    // check for joint jump and if not detected execute motion
-//                    if (checkJointJump(boost::shared_ptr<moveit_msgs::RobotTrajectory>(&traj), 90.0)){
+                    if (checkJointJump(&traj, robot_data.joint_jump_tresh)){
-//                        ROS_WARN("Joint Jump Detected!!!");
+                        ROS_WARN("Joint Jump Detected!!! \n ABORTING EXECUTION ");
-//                    }
+                    } else {
                         jointPlan.trajectory_ = traj;
                         plotTrajectory(&rviz_data, &robot_data, &traj);
                         // async execute planned trajectory
                         moveit::planning_interface::MoveItErrorCode retval = move_group.execute(jointPlan);
                         ROS_INFO("Finished executing first [%u] datapoints, execute: %d", i, retval.val);
+                    }
                 } else if (!robot_data.planning_mode.compare("joint")){
-                    // TODO SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO
+                    // TODO: SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO
                     // QUANDO I PROBLEMI DI STABILITÀ DEL MOVIMENTO SONO RISOLTI
                 } else {
                     ROS_ERROR("Invalid Plannning Mode!");

src/roboglue_ros/src/roboglue_utils.cpp

@@ -94,7 +94,7 @@ double deg2rad(double ang){
     return ang*M_PI/180;
 }
 
-bool checkJointJump(moveit_msgs::RobotTrajectoryPtr traj, double thresh){
+bool checkJointJump(moveit_msgs::RobotTrajectory *traj, double thresh){
     // identify max joint jump for each joint
     typedef struct{
         double first, second;