Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Detection dei salti di giunto funzionante

Browse Source
This commit is contained in:
Emanuele
2019-10-28 14:27:45 +01:00
parent c56b427e47
commit 7e9dbe38e1
6 changed files with 19 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
View File

@@ -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);

1
src/roboglue_ros/launch/roboglue_ros.launch
View File

@@ -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>

1
src/roboglue_ros/launch/roboglue_ros_urdriver.launch
View File

@@ -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>

1
src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch
View File

@@ -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" />

23
src/roboglue_ros/src/roboglue_recorder.cpp
View File

@@ -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.
// if (checkJointJump(boost::shared_ptr<moveit_msgs::RobotTrajectory>(&traj), 90.0)){
// ROS_WARN("Joint Jump Detected!!!");
// }
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);
// check for joint jump and if not detected execute motion
if (checkJointJump(&traj, robot_data.joint_jump_tresh)){
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!");

2
src/roboglue_ros/src/roboglue_utils.cpp
View File

@@ -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;