00001 #include <ee_cart_imped_action/ee_cart_imped_arm.hh> 00002 #include <geometry_msgs/PoseStamped.h> 00003 #include <iostream> 00004 00009 int main(int argc, char **argv) { 00010 ros::init(argc, argv, "ee_cart_imped_action_cpp_testing_node"); 00011 00012 EECartImpedArm right_arm("r_arm_cart_imped_controller"), 00013 left_arm("l_arm_cart_imped_controller"); 00014 ee_cart_imped_msgs::EECartImpedGoal r_traj, l_traj; 00015 00016 ROS_INFO("The ee_cart_imped_action does not check for collisions. Move the robot away from obstacles and press enter when ready to proceed."); 00017 std::string input; 00018 getline(std::cin, input); 00019 00020 ROS_INFO("Moving to initial positions"); 00021 00022 EECartImpedArm::addTrajectoryPoint(r_traj, 0, -0.4, -0.2, 00023 0, 0.707, 0, 0.707, 00024 1000, 1000, 1000, 100, 100, 100, 00025 false, false, false, false, false, false, 00026 4.0, "/torso_lift_link"); 00027 right_arm.startTrajectory(r_traj); 00028 EECartImpedArm::addTrajectoryPoint(l_traj, 0, 0.4, -0.2, 00029 0, 0.707, 0, 0.707, 00030 1000, 1000, 1000, 100, 100, 100, 00031 false, false, false, false, false, false, 00032 4.0, "/torso_lift_link"); 00033 left_arm.startTrajectory(l_traj); 00034 r_traj.trajectory.clear(); 00035 l_traj.trajectory.clear(); 00036 ROS_INFO("Beginning trajectory tests"); 00037 for (int i = 0; i < 2; i++) { 00038 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0.707, 0, 0, 0.707, 00039 8, 1000, 5, 100, 100, 100, 00040 true, false, true, false, false, false, 00041 4.0, "/base_link"); 00042 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0.707, 0, 0, 0.707, 00043 -3, 1000, 5, 100, 100, 100, 00044 true, false, true, false, false, false, 00045 6.0, "/base_link"); 00046 ROS_INFO("Press enter for a force trajectory of 2 points on the right arm"); 00047 getline(std::cin, input); 00048 right_arm.startTrajectory(r_traj); 00049 r_traj.trajectory.clear(); 00050 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 2.0, 0.707, 0, 0, 0.707, 00051 1000, 1000, 1000, 1.0, 100, 100, 00052 false, false, false, true, false, false, 00053 3.0, "/base_link"); 00054 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0.5, 0.5, 0.5, 0.5, 00055 1000, 1000, 1000, 100, 100, 100, 00056 false, false, false, false, false, false, 00057 5.0, "/base_link"); 00058 EECartImpedArm::addTrajectoryPoint(r_traj, 00059 0.2, -0.6, 1.0, -0.5, 0.5, 0.5, 0.5, 00060 1000, 1000, 1000, 100, 100, 100, 00061 false, false, false, false, false, false, 00062 9.0, "/base_link"); 00063 00064 ROS_INFO("Press enter for a torque to stiffness trajectory on the right arm"); 00065 getline(std::cin, input); 00066 right_arm.startTrajectory(r_traj); 00067 r_traj.trajectory.clear(); 00068 EECartImpedArm::addTrajectoryPoint(l_traj, 0, 0.4, 0, 0.707, 0, 0, 0.707, 00069 1000, 1000, 1000, 100, 100, 100, 00070 false, false, false, false, false, false, 00071 2.0, "/torso_lift_link"); 00072 EECartImpedArm::addTrajectoryPoint(l_traj, 0, 0.4, 0, 0.707, 0, 0, 0.707, 00073 -5, 1000, 1000, 100, 100, 100, 00074 true, false, false, false, false, false, 00075 4.0, "/torso_lift_link"); 00076 EECartImpedArm::addTrajectoryPoint(l_traj, 00077 0.1, 0.4, -0.2, 0.707, 0, 0, 0.707, 00078 1000, 1000, 1000, 100, 100, 100, 00079 false, false, false, false, false, false, 00080 6.0, "/torso_lift_link"); 00081 ROS_INFO("Press enter for a trajectory on the left arm"); 00082 getline(std::cin, input); 00083 left_arm.startTrajectory(l_traj); 00084 l_traj.trajectory.clear(); 00085 00086 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0.707, 0, 0, 0.707, 00087 1000, 1000, 1000, 100, 100, 100, 00088 false, false, false, false, false, false, 00089 2.0, "/base_link"); 00090 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 0.0, 0.707, 0, 0, 0.707, 00091 1000, -10, 1000, 100, 100, 100, 00092 false, true, false, false, false, false, 00093 4.0, "/torso_lift_link"); 00094 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0.707, 0, 0, 0.707, 00095 1000, 1000, -5, 100, 100, 100, 00096 false, false, true, false, false, false, 00097 6.0, "/base_link"); 00098 EECartImpedArm::addTrajectoryPoint(r_traj, 0.1, -0.4, 1.0, 0, 0, 0, 1.0, 00099 1000, 1000, 1000, 100, 100, 100, 00100 false, false, false, false, false, false, 00101 8.0, "/base_link"); 00102 ROS_INFO("Press enter for a mixed trajectory on right arm"); 00103 getline(std::cin, input); 00104 right_arm.startTrajectory(r_traj); 00105 r_traj.trajectory.clear(); 00106 } 00107 00108 ROS_INFO("Beginning canceling tests"); 00109 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0, 0, 0, 1, 00110 1000, 1000, 1000, 100, 100, 100, 00111 false, false, false, false, false, false, 00112 4.0, "/base_link"); 00113 right_arm.startTrajectory(r_traj); 00114 r_traj.trajectory.clear(); 00115 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0.707, 0, 0, 0.707, 00116 1000, 1000, -5, 100, 100, 100, 00117 false, false, true, false, false, false, 00118 2.0, "/base_link"); 00119 EECartImpedArm::addTrajectoryPoint(r_traj, 0.6, 0, 1.0, 0, 0, 0, 1.0, 00120 1000, 1000, 1000, 100, 100, 100, 00121 false, false, false, false, false, false, 00122 4.0, "/base_link"); 00123 ROS_INFO("Press enter to test canceling goal"); 00124 getline(std::cin, input); 00125 right_arm.startTrajectory(r_traj, false); 00126 right_arm.stopTrajectory(); 00127 ROS_INFO("Press enter to test canceling goal on second trajectory point"); 00128 getline(std::cin, input); 00129 right_arm.startTrajectory(r_traj, false); 00130 ros::Duration(2.5).sleep(); 00131 right_arm.stopTrajectory(); 00132 ROS_INFO("Returning to initial position"); 00133 right_arm.startTrajectory(r_traj); 00134 ROS_INFO("Press enter to test goal preempt"); 00135 getline(std::cin, input); 00136 right_arm.startTrajectory(r_traj, false); 00137 ros::Duration(2.5).sleep(); 00138 right_arm.startTrajectory(r_traj); 00139 ROS_INFO("Press enter to test quick succession of goals."); 00140 getline(std::cin, input); 00141 for (int i = 0; i < 50; i++) { 00142 right_arm.startTrajectory(r_traj, false); 00143 ros::Duration(0.1).sleep(); 00144 } 00145 ROS_INFO("Returning to initial position"); 00146 right_arm.startTrajectory(r_traj); 00147 r_traj.trajectory.clear(); 00148 EECartImpedArm::addTrajectoryPoint(r_traj, 0, -0.4, -0.2, 0, 0.707, 0, 0.707, 00149 1000, 1000, 1000, 100, 100, 100, 00150 false, false, false, false, false, false, 00151 4.0, "/torso_lift_link"); 00152 EECartImpedArm::addTrajectoryPoint(l_traj, 0, 0.4, -0.2, 0, 0.707, 0, 0.707, 00153 1000, 1000, 1000, 100, 100, 100, 00154 false, false, false, false, false, false, 00155 4.0, "/torso_lift_link"); 00156 ROS_INFO("Press enter to test simultaneous arm movement"); 00157 getline(std::cin, input); 00158 left_arm.startTrajectory(l_traj, false); 00159 right_arm.startTrajectory(r_traj); 00160 ROS_INFO("Tests completed"); 00161 } 00162