added accuracy test for moveit/gazebo motion

added accuracy test for moveit/gazebo motion
removed goal duration check from MoveIt

kinova_control/src/move_robot.py

@@ -25,7 +25,7 @@ def moveJoint (jointcmds,prefix,nbJoints):
   jointCmd = JointTrajectory()  
   point = JointTrajectoryPoint()
   jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
-  point.time_from_start = rospy.Duration.from_sec(10.0)
+  point.time_from_start = rospy.Duration.from_sec(5.0)
   for i in range(0, nbJoints):
     jointCmd.joint_names.append(prefix +'_joint_'+str(i+1))
     point.positions.append(jointcmds[i])
@@ -66,13 +66,13 @@ def moveFingers (jointcmds,prefix,nbJoints):
     rospy.init_node('move_robot_using_trajectory_msg')		
     prefix, nbJoints, nbfingers = argumentParser(None)    
     #allow gazebo to launch
-    rospy.sleep(5)
+    rospy.sleep(1)
 
     if (nbJoints==6):
       #home robots
       moveJoint ([0.0,2.9,1.3,4.2,1.4,0.0],prefix,nbJoints)
     else:
-      moveJoint ([0.0,2.9,0.0,1.3,4.2,4.4,0.0],prefix,nbJoints)
+      moveJoint ([0.0,2.9,0.0,1.3,4.2,1.4,0.0],prefix,nbJoints)
 
     moveFingers ([1,1,1],prefix,nbfingers)
   except rospy.ROSInterruptException:

kinova_moveit/kinova_arm_moveit_demo/CMakeLists.txt

@@ -53,6 +53,10 @@ add_executable(pick_place src/pick_place.cpp include/pick_place.h)
 target_link_libraries(pick_place ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${kinova_driver_LIBRARIES})
 install(TARGETS pick_place DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
+add_executable(test_accuracy src/test_accuracy.cpp include/test_accuracy.h)
+target_link_libraries(test_accuracy ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${kinova_driver_LIBRARIES})
+install(TARGETS test_accuracy DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
 
 #get_cmake_property(_variableNames VARIABLES)
 #foreach (_variableName ${_variableNames})

kinova_moveit/kinova_arm_moveit_demo/include/test_accuracy.h

@@ -0,0 +1,137 @@
+#ifndef TEST_ACCURACY_H
+#define TEST_ACCURACY_H
+
+#include <ros/ros.h>
+#include <kinova_driver/kinova_ros_types.h>
+
+#include <actionlib/client/simple_action_client.h>
+#include <kinova_msgs/SetFingersPositionAction.h>
+
+// MoveIt!
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_state/conversions.h>
+
+#include <moveit/move_group_interface/move_group.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/planning_pipeline/planning_pipeline.h>
+
+#include <moveit/kinematic_constraints/utils.h>
+#include <geometric_shapes/solid_primitive_dims.h>
+
+#include <moveit_msgs/PlanningScene.h>
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/GetStateValidity.h>
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+#include <fstream>
+#include <pwd.h>
+#include <eigen_conversions/eigen_msg.h>
+
+namespace kinova
+{
+
+
+    class PickPlace
+    {
+    public:
+        PickPlace(ros::NodeHandle &nh);
+        ~PickPlace();
+
+
+
+    private:
+        ros::NodeHandle nh_;
+
+        // open&close fingers: gripper_group_.plan not alway have a solution
+        actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction>* finger_client_;
+
+        moveit::planning_interface::MoveGroup* group_;
+        moveit::planning_interface::MoveGroup* gripper_group_;
+        robot_model::RobotModelPtr robot_model_;
+//        robot_state::RobotStatePtr robot_state_;
+
+        planning_scene::PlanningScenePtr planning_scene_;
+        planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+
+        // work scene
+        moveit_msgs::CollisionObject co_;
+        moveit_msgs::AttachedCollisionObject aco_;
+        moveit_msgs::PlanningScene planning_scene_msg_;
+
+
+        ros::Publisher pub_co_;
+        ros::Publisher pub_aco_;
+        ros::Publisher pub_planning_scene_diff_;
+        ros::Subscriber sub_pose_;
+        ros::Subscriber sub_joint_;
+
+        //
+        std::vector<std::string> joint_names_;
+        std::vector<double> joint_values_;
+
+        // use Kinova Inverse Kinematic model to generate joint value, then setJointTarget().
+        bool use_KinovaInK_;
+
+        // check some process if success.
+        bool result_;
+        // wait for user input to continue: cin >> pause_;
+        std::string pause_;
+        std::string robot_type_;
+        bool robot_connected_;
+
+        // update current state and pose
+        boost::mutex mutex_state_;
+        boost::mutex mutex_pose_;
+        sensor_msgs::JointState current_state_;
+        geometry_msgs::PoseStamped current_pose_;
+
+
+        // define pick_place joint value and pose
+        std::vector<double> start_joint_;
+        std::vector<double> grasp_joint_;
+        std::vector<double> pregrasp_joint_;
+        std::vector<double> postgrasp_joint_;
+
+        geometry_msgs::PoseStamped start_pose_;
+        geometry_msgs::PoseStamped grasp_pose_;
+        geometry_msgs::PoseStamped can_pose_;
+        geometry_msgs::PoseStamped pregrasp_pose_;
+        geometry_msgs::PoseStamped postgrasp_pose_;
+
+        bool cartesian_move_;
+        std::ofstream o_file_;
+
+        void build_workscene();
+        void add_obstacle();
+        void add_complex_obstacle();
+        void clear_obstacle();
+        void clear_workscene();
+        void add_attached_obstacle();
+        void add_target();
+
+        void define_joint_values();
+        void define_cartesian_pose();
+        geometry_msgs::PoseStamped generate_gripper_align_pose(geometry_msgs::PoseStamped targetpose_msg, double dist, double azimuth, double polar, double rot_gripper_z);
+        void setup_constrain(geometry_msgs::Pose target_pose, bool orientation, bool position);
+        void check_constrain();
+
+        bool my_pick();
+        bool my_place();
+
+        void get_current_state(const sensor_msgs::JointStateConstPtr &msg);
+        void get_current_pose(const geometry_msgs::PoseStampedConstPtr &msg);
+        // TODO: use Kinova inverse kinematic solution instead of from ROS.
+        void getInvK(geometry_msgs::Pose &eef_pose, std::vector<double> &joint_value);
+        void check_collision();
+        void evaluate_plan(moveit::planning_interface::MoveGroup &group);
+        bool gripper_action(double gripper_rad);
+        void evaluate_move_accuracy();
+    };
+}
+
+
+#endif // TEST_ACCURACY

kinova_moveit/kinova_arm_moveit_demo/src/pick_place.cpp

@@ -427,8 +427,6 @@ void PickPlace::define_cartesian_pose()
     start_pose_.pose.orientation.y = q.y();
     start_pose_.pose.orientation.z = q.z();
     start_pose_.pose.orientation.w = q.w();
-    ROS_WARN_STREAM(__PRETTY_FUNCTION__ << ": LINE: " << __LINE__ << ": " <<
-                    std::endl << "start_pose_ q = EulerZYZ_to_Quaternion(-M_PI/4, M_PI/2, M_PI/2), qx: " << q.x() << ", q.y: " << q.y() << ", q.z: " << q.z() << ", q.w: " << q.w());
 
     // define grasp pose
     grasp_pose_.header.frame_id = "root";
@@ -444,8 +442,6 @@ void PickPlace::define_cartesian_pose()
     grasp_pose_.pose.orientation.y = q.y();
     grasp_pose_.pose.orientation.z = q.z();
     grasp_pose_.pose.orientation.w = q.w();
-    ROS_WARN_STREAM(__PRETTY_FUNCTION__ << ": LINE: " << __LINE__ << ": " << std::endl << "grasp_pose_ q = EulerZYZ_to_Quaternion(M_PI/4, M_PI/2, M_PI/2), qx: " << q.x() << ", q.y: " << q.y() << ", q.z: " << q.z() << ", q.w: " << q.w());
-
 
     // generate_pregrasp_pose(double dist, double azimuth, double polar, double rot_gripper_z)
     grasp_pose_= generate_gripper_align_pose(grasp_pose_, 0.03999, M_PI/4, M_PI/2, M_PI);
@@ -796,8 +792,7 @@ bool PickPlace::my_pick()
     group_->setPoseTarget(grasp_pose_);
     evaluate_plan(*group_);
 
-    ROS_INFO_STREAM("Press any key to grasp ...");
-    std::cin >> pause_;
+    ROS_INFO_STREAM("Grasping ...");
     add_attached_obstacle();
     gripper_action(0.75*FINGER_MAX); // partially close