move_arm: move_arm: regression_test_arm

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  *********************************************************************/
00036 
00037 /* \author: E. Gil Jones */
00038 
00039 #ifndef REGRESSION_TEST_ARM_HELPERS
00040 #define REGRESSION_TEST_ARM_HELPERS
00041 
00042 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00043 #include <planning_environment/util/kinematic_state_constraint_evaluator.h>
00044 #include <move_arm/MoveArmAction.h>
00045 
00046 #include <planning_models/kinematic_state.h>
00047 
00048 void setupGoal(const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
00049 {
00050     goal.goal_constraints.joint_constraint.resize(names.size());
00051     for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
00052     {
00053         goal.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
00054         goal.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
00055         goal.goal_constraints.joint_constraint[i].joint_name = names[i];
00056         goal.goal_constraints.joint_constraint[i].value.resize(1);
00057         goal.goal_constraints.joint_constraint[i].tolerance_above.resize(1);
00058         goal.goal_constraints.joint_constraint[i].tolerance_below.resize(1);
00059         goal.goal_constraints.joint_constraint[i].value[0] = 0.0;
00060         goal.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
00061         goal.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
00062     }
00063 
00064     goal.contacts.resize(1);
00065     goal.contacts[0].links.push_back("r_gripper_l_finger_link");
00066     goal.contacts[0].links.push_back("r_gripper_r_finger_link");
00067     goal.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
00068     goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
00069     goal.contacts[0].links.push_back("r_gripper_palm_link");
00070     goal.contacts[0].links.push_back("r_wrist_roll_link");
00071     goal.contacts[0].depth = 0.04;
00072     goal.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
00073     goal.contacts[0].bound.dimensions.push_back(0.5);
00074 
00075     goal.contacts[0].pose.header.stamp = ros::Time::now();
00076     goal.contacts[0].pose.header.frame_id = "/base_link";
00077     goal.contacts[0].pose.pose.position.x = 1;
00078     goal.contacts[0].pose.pose.position.y = 0;
00079     goal.contacts[0].pose.pose.position.z = 0.5;
00080 
00081     goal.contacts[0].pose.pose.orientation.x = 0;
00082     goal.contacts[0].pose.pose.orientation.y = 0;
00083     goal.contacts[0].pose.pose.orientation.z = 0;
00084     goal.contacts[0].pose.pose.orientation.w = 1;
00085 }
00086 
00087 void goalToState(const move_arm::MoveArmGoal &goal, planning_models::KinematicState &sp)
00088 {
00089     for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size() ; ++i)
00090     {
00091         sp.setParamsJoint(&goal.goal_constraints.joint_constraint[i].value[0],
00092                           goal.goal_constraints.joint_constraint[i].joint_name);
00093     }
00094 }
00095 
00096 btTransform effPosition(const planning_environment::KinematicModelStateMonitor &km, const move_arm::MoveArmGoal &goal)
00097 {
00098     planning_models::KinematicState sp(*km.getRobotState());
00099     goalToState(goal, sp);
00100     km.getKinematicModel()->computeTransforms(sp.getParams());
00101     return km.getKinematicModel()->getJoint("r_wrist_roll_joint")->after->globalTrans;
00102 }
00103 
00104 void diffConfig(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal, double& dist, double& angle)
00105 {
00106   //getting goal pose from km, I hope
00107   btTransform pose1 = effPosition(km, goal);
00108   
00109   //getting current pose from km
00110   std::vector<std::string> names(7);
00111   names[0] = "r_shoulder_pan_joint";
00112   names[1] = "r_shoulder_lift_joint";
00113   names[2] = "r_upper_arm_roll_joint";
00114   names[3] = "r_elbow_flex_joint";
00115   names[4] = "r_forearm_roll_joint";
00116   names[5] = "r_wrist_flex_joint";
00117   names[6] = "r_wrist_roll_joint";
00118   move_arm::MoveArmGoal temp;
00119   setupGoal(names,temp);
00120 
00121   planning_models::KinematicState sp(*(km.getRobotState()));
00122   sp.enforceBounds();
00123   for (unsigned int i = 0 ; i < names.size() ; ++i)
00124   {
00125     temp.goal_constraints.joint_constraint[i].value[0] =
00126       sp.getParamsJoint(names[i])[0];
00127   }
00128 
00129   btTransform pose2 = effPosition(km, temp);
00130   dist = pose1.getOrigin().distance(pose2.getOrigin());
00131   angle = pose1.getRotation().angle(pose2.getRotation());
00132 }
00133 
00134 // bool checkCurrentStateVersusEffPoseConstraint(const planning_environment::KinematicModelStateMonitor &km, const arm_navigation_msgs::PoseConstraint& constraints) {
00135   
00136 //   for (unsigned int i = 0 ; i < constraints.joint_constraint.size() ; ++i)
00137 //   {
00138 //     sp.setParamsJoint(&constraints.joint_constraint[i].value[0],
00139 //                       constraints.joint_constraint[i].joint_name);
00140 //   }
00141  
00142 //   km.getKinematicModel()->computeTransforms(sp.getParams());
00143 //   btTransform pose1 = km.getKinematicModel()->getJoint("r_wrist_roll_joint")->after->globalTrans;
00144   
00145 //   planning_models::KinematicState sp(*km.getRobotState());
00146 //   move_arm::MoveArmGoal temp;
00147 //   setupGoal(names,temp);
00148 
00149 //   planning_models::KinematicState sp(*(km.getRobotState()));
00150 //   sp.enforceBounds();
00151 //   for (unsigned int i = 0 ; i < names.size() ; ++i)
00152 //   {
00153 //     temp.goal_constraints.joint_constraint[i].value[0] =
00154 //       sp.getParamsJoint(names[i])[0];
00155 //   }
00156 
00157 //   btTransform pose2 = effPosition(km, temp);
00158   
00159 //   btVector3 origin1 = pose1.getOrigin();
00160 //   btVector3 origin2 = pose2.getOrigin();
00161 // }
00162 
00163 bool finalStateMatchesGoal(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal) 
00164 {
00165   planning_models::KinematicState sp(*km.getRobotState());
00166 
00167   km.getKinematicModel()->computeTransforms(sp.getParams());
00168 
00169   planning_environment::KinematicConstraintEvaluatorSet ks;
00170   ks.add(km.getKinematicModel(),goal.goal_constraints.joint_constraint);
00171   ks.add(km.getKinematicModel(),goal.goal_constraints.pose_constraint);
00172 
00173   ks.print(std::cout);
00174   
00175   return ks.decide(sp.getParams(),true);
00176   
00177 }
00178 
00179 bool currentStateSatisfiesPathConstraints(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal) {
00180 
00181   planning_models::KinematicState sp(*km.getRobotState());
00182 
00183   km.getKinematicModel()->computeTransforms(sp.getParams());
00184  
00185   planning_environment::KinematicConstraintEvaluatorSet ks;
00186   ks.add(km.getKinematicModel(),goal.path_constraints.joint_constraint);
00187   ks.add(km.getKinematicModel(),goal.path_constraints.pose_constraint);
00188 
00189   ks.print(std::cout);
00190 
00191   bool ok = ks.decide(sp.getParams(),true);
00192   
00193   if(!ok) {
00194     //to get a print out
00195     ks.decide(sp.getParams(),true);
00196   }
00197 
00198   return ok;
00199 }
00200 
00201 #endif
regression_test_arm_helpers.h
