Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <ros/ros.h>
00040 #include <tf/tf.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <move_arm/MoveArmAction.h>
00043 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00044 #include <planning_models/kinematic_state.h>
00045
00046 #include <stdio.h>
00047 #include <stdlib.h>
00048 #include <time.h>
00049 #include <boost/thread.hpp>
00050 #include <ros/ros.h>
00051 #include <gtest/gtest.h>
00052
00053 void setupGoal(const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
00054 {
00055 goal.goal_constraints.joint_constraint.resize(names.size());
00056 for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
00057 {
00058 goal.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
00059 goal.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
00060 goal.goal_constraints.joint_constraint[i].joint_name = names[i];
00061 goal.goal_constraints.joint_constraint[i].value.resize(1);
00062 goal.goal_constraints.joint_constraint[i].tolerance_above.resize(1);
00063 goal.goal_constraints.joint_constraint[i].tolerance_below.resize(1);
00064 goal.goal_constraints.joint_constraint[i].value[0] = 0.0;
00065 goal.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
00066 goal.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
00067 }
00068
00069 goal.contacts.resize(1);
00070 goal.contacts[0].links.push_back("r_gripper_l_finger_link");
00071 goal.contacts[0].links.push_back("r_gripper_r_finger_link");
00072 goal.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
00073 goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
00074 goal.contacts[0].links.push_back("r_gripper_palm_link");
00075 goal.contacts[0].links.push_back("r_wrist_roll_link");
00076 goal.contacts[0].depth = 0.04;
00077 goal.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
00078 goal.contacts[0].bound.dimensions.push_back(0.5);
00079
00080 goal.contacts[0].pose.header.stamp = ros::Time::now();
00081 goal.contacts[0].pose.header.frame_id = "/base_link";
00082 goal.contacts[0].pose.pose.position.x = 1;
00083 goal.contacts[0].pose.pose.position.y = 0;
00084 goal.contacts[0].pose.pose.position.z = 0.5;
00085
00086 goal.contacts[0].pose.pose.orientation.x = 0;
00087 goal.contacts[0].pose.pose.orientation.y = 0;
00088 goal.contacts[0].pose.pose.orientation.z = 0;
00089 goal.contacts[0].pose.pose.orientation.w = 1;
00090 }
00091
00092 void goalToState(const move_arm::MoveArmGoal &goal, planning_models::KinematicState &sp)
00093 {
00094 for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size() ; ++i)
00095 {
00096 sp.setParamsJoint(&goal.goal_constraints.joint_constraint[i].value[0],
00097 goal.goal_constraints.joint_constraint[i].joint_name);
00098 }
00099 }
00100
00101 tf::Transform effPosition(const planning_environment::KinematicModelStateMonitor &km, const move_arm::MoveArmGoal &goal)
00102 {
00103 planning_models::KinematicState sp(*km.getRobotState());
00104 goalToState(goal, sp);
00105 km.getKinematicModel()->computeTransforms(sp.getParams());
00106 return km.getKinematicModel()->getJoint("r_wrist_roll_joint")->after->globalTrans;
00107 }
00108
00109 void diffConfig(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal)
00110 {
00111 std::vector<std::string> names(7);
00112 names[0] = "r_shoulder_pan_joint";
00113 names[1] = "r_shoulder_lift_joint";
00114 names[2] = "r_upper_arm_roll_joint";
00115 names[3] = "r_elbow_flex_joint";
00116 names[4] = "r_forearm_roll_joint";
00117 names[5] = "r_wrist_flex_joint";
00118 names[6] = "r_wrist_roll_joint";
00119 for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
00120 {
00121 std::cout << " " << goal.goal_constraints.joint_constraint[i].joint_name << " = "
00122 << goal.goal_constraints.joint_constraint[i].value[0] - km.getRobotState()->getParamsJoint(goal.goal_constraints.joint_constraint[i].joint_name)[0]
00123 << std::endl;
00124 }
00125
00126 tf::Transform pose1 = effPosition(km, goal);
00127 move_arm::MoveArmGoal temp;
00128
00129 planning_models::KinematicState sp(*(km.getRobotState()));
00130 sp.enforceBounds();
00131 for (unsigned int i = 0 ; i < names.size() ; ++i)
00132 {
00133 temp.goal_constraints.joint_constraint[i].value[0] =
00134 sp.getParamsJoint(temp.goal_constraints.joint_constraint[i].joint_name)[0];
00135 }
00136 tf::Transform pose2 = effPosition(km, temp);
00137 std::cout << std::endl;
00138 double dist = pose1.getOrigin().distance(pose2.getOrigin());
00139 std::cout << " -position distance: " << dist << std::endl;
00140 double angle = pose1.getRotation().angle(pose2.getRotation());
00141 std::cout << " -rotation distance: " << angle << std::endl;
00142 }
00143
00144 void spinThread()
00145 {
00146 ros::spin();
00147 }
00148
00149 TEST(MoveArm, goToPoseGoal)
00150 {
00151 ros::NodeHandle nh;
00152 actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_right_arm");
00153 boost::thread spin_thread(&spinThread);
00154
00155 nh.setParam( "/move_right_arm/long_range_only", true);
00156
00157 move_arm.waitForServer();
00158 ROS_INFO("Connected to server");
00159 move_arm::MoveArmGoal goalA;
00160
00161
00162 planning_environment::RobotModels rm("robot_description");
00163 EXPECT_TRUE(rm.loadedModels());
00164
00165 tf::TransformListener tf;
00166 planning_environment::KinematicModelStateMonitor km(&rm, &tf);
00167 km.waitForState();
00168
00169
00170 goalA.goal_constraints.set_pose_constraint_size(1);
00171 goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
00172 goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
00173
00174
00175
00176
00177 goalA.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X
00178 + arm_navigation_msgs::PoseConstraint::POSITION_Y
00179 + arm_navigation_msgs::PoseConstraint::POSITION_Z
00180 + arm_navigation_msgs::PoseConstraint::ORIENTATION_R
00181 + arm_navigation_msgs::PoseConstraint::ORIENTATION_P
00182 + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
00183
00184 goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
00185 goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.413446;
00186 goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = 0.0124666;
00187 goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.206998;
00188
00189 goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = .132744;
00190 goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = .888279;
00191 goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = .288412;
00192 goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = .331901;
00193
00194 goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
00195 goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
00196 goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
00197 goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
00198 goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
00199 goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
00200
00201 goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
00202 goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.005;
00203 goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
00204 goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
00205 goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.005;
00206 goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.005;
00207
00208 goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
00209 goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
00210 goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
00211 goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
00212 goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
00213 goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
00214
00215 goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
00216
00217 if (nh.ok())
00218 {
00219 bool finished_within_time = false;
00220 move_arm.sendGoal(goalA);
00221 finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
00222
00223 diffConfig(km,goalA);
00224
00225 EXPECT_TRUE(finished_within_time);
00226 if (!finished_within_time)
00227 {
00228 move_arm.cancelGoal();
00229 ROS_INFO("Timed out achieving goal A");
00230 }
00231 else
00232 {
00233 actionlib::SimpleClientGoalState state = move_arm.getState();
00234 ROS_INFO("Action finished: %s",state.toString().c_str());
00235 }
00236 }
00237
00238
00239 diffConfig(km,goalA);
00240
00241 ros::shutdown();
00242 spin_thread.join();
00243 }
00244
00245 int main(int argc, char **argv){
00246 testing::InitGoogleTest(&argc, argv);
00247 ros::init (argc, argv, "move_arm_regression_test");
00248 return RUN_ALL_TESTS();
00249 }