Why r_arm_controller may stuck sometimes? [closed]

edit

It works as normal for many points, and failed at some points that I don't know why.

Execution situation:

[ INFO] [1404408689.061515355]: Get Solver INFO
[ INFO] [1404408689.066450708]: IK Computation Succeeded
[ INFO] [1404408689.359426103, 518.289000000]: Action state : SUCCEEDED

[ INFO] [1404408689.831880522]: Get Solver INFO
[ INFO] [1404408689.834025237]: IK Computation Succeeded
[ INFO] [1404408695.637234939, 521.075000000]: Action state : SUCCEEDED

[ INFO] [1404408696.301499841]: Get Solver INFO
[ INFO] [1404408696.303130034]: IK Computation Succeeded

It stops at the last line.

My script:

while true
do
    rosrun sam_controller_ik_move_right_hand move_hand 0.5 0.0 0.3 
    rosrun sam_controller_ik_move_right_hand move_hand 0.5 -0.2 0.3 
    rosrun sam_controller_ik_move_right_hand move_hand 0.5 -0.2 0.5 
    rosrun sam_controller_ik_move_right_hand move_hand 0.5 0.0 0.5 
done

And my move_hand code:

#include "ros/ros.h"

#include "kinematics_msgs/GetKinematicSolverInfo.h"
#include "kinematics_msgs/GetPositionIK.h"

#include "pr2_controllers_msgs/JointTrajectoryAction.h"
#include "actionlib/client/simple_action_client.h"

const std::string CONTROLLER = "r_arm_controller/joint_trajectory_action";
const std::string SOLVER_INFO = "pr2_right_arm_kinematics/get_ik_solver_info";
const std::string IK_SOLVER = "pr2_right_arm_kinematics/get_ik";

int main(int argc, char** argv)
{
    ros::init(argc, argv, "move_hand");
    ros::NodeHandle nh;

    ros::service::waitForService(SOLVER_INFO);
    ros::service::waitForService(IK_SOLVER);

    ros::ServiceClient solver_client = nh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(SOLVER_INFO);
    ros::ServiceClient ik_client = nh.serviceClient<kinematics_msgs::GetPositionIK>(IK_SOLVER);

    kinematics_msgs::GetKinematicSolverInfo::Request solver_req;
    kinematics_msgs::GetKinematicSolverInfo::Response solver_res;
    if(solver_client.call(solver_req, solver_res))
        ROS_INFO("Get Solver INFO");
    else
    {
        ROS_INFO("Can't Get Solver INFO");
        ros::shutdown();
    }

    kinematics_msgs::GetPositionIK::Request ik_req;
    kinematics_msgs::GetPositionIK::Response ik_res;

    ik_req.timeout = ros::Duration(0.2);//允許計算IK的時間
    ik_req.ik_request.ik_link_name = solver_res.kinematic_solver_info.link_names[0];//在這邊的值是
    ik_req.ik_request.pose_stamped.header.frame_id = "base_link";//x,y,z的reference frame

    //設定x,y,z
    ik_req.ik_request.pose_stamped.pose.position.x = atof(argv[1]);
    ik_req.ik_request.pose_stamped.pose.position.y = atof(argv[2]);
    ik_req.ik_request.pose_stamped.pose.position.z = atof(argv[3]);

    //設定quaternion
    ik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
    ik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
    ik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
    ik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;

    ik_req.ik_request.ik_seed_state.joint_state.name = solver_res.kinematic_solver_info.joint_names;//要計算的joint
    for(unsigned int i = 0; i < solver_res.kinematic_solver_info.joint_names.size(); i++)
    {
        //seed是猜ik解大概會落在哪個範圍，不知道就猜join position的max和min中間值
        ik_req.ik_request.ik_seed_state.joint_state.position.push_back(
                (solver_res.kinematic_solver_info.limits[i].min_position + solver_res.kinematic_solver_info.limits[i].max_position)/2);
    }
    ik_client.call(ik_req, ik_res);

    if(ik_res.error_code.val != 1)
    {
        ROS_INFO("IK Computation Failed!!");
        ros::shutdown();
    }
    else
        ROS_INFO("IK Computation Succeeded");

    actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> controller(CONTROLLER, true);
    controller.waitForServer();

    pr2_controllers_msgs::JointTrajectoryGoal goal;
    goal.trajectory.joint_names = ik_res.solution.joint_state.name;
    goal.trajectory.points.resize(1);
    goal.trajectory.points[0].positions.resize(ik_res.solution.joint_state.name.size());
    goal.trajectory.points[0].velocities.resize(ik_res.solution.joint_state.name.size());
    for(unsigned int i = 0; i < ik_res.solution.joint_state.name.size(); i++)
    {
        //把IK算出來的joint value填入
        goal.trajectory.points[0].positions[i] = ik_res.solution.joint_state.position[i];

        //velocity=0.0代表不設限，這個參數的作用還沒有搞得很清楚
        //只知道如果不是設為0.0移動的方式會很詭異
        goal.trajectory.points[0].velocities[i] = 0.0;
    }

    //真正移動手臂的時間範圍，實測發現這個跟移動速度比較有關係
    goal.trajectory.points[0].time_from_start = ros::Duration(2.0);

    goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.5);//什麼時候開始移動

    ROS_INFO("Action state : %s\n", controller.sendGoalAndWait(goal).toString ...