problem about getting joint rotation axis in Moveit! [closed]

edit

Hi everyone,

I tried to use moveit API to get joint rotation axis for different states of robot. However, I keep getting the same joint rotation axis vector for different states. I edit the code here(line 140) by following:

void move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal, moveit_msgs::MoveGroupResult &action_res)
{
  ROS_INFO("Planning request received for MoveGroup action. Forwarding to planning pipeline.");

  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); // lock the scene so that it does not modify the world representation while diff() is called
  const planning_scene::PlanningSceneConstPtr &the_scene = (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) : lscene->diff(goal->planning_options.planning_scene_diff);
  planning_interface::MotionPlanResponse res;

  try
  {
    collision_detection::CollisionRequest collision_request;
    collision_detection::CollisionResult collision_result;
    robot_state::RobotState copied_state = the_scene->getCurrentState();

    const robot_model::RobotModelConstPtr& copied_model = copied_state.getRobotModel();
    const robot_model::JointModel* copied_joint_model = copied_model->getJointModel("r_wrist_roll_joint");
    const robot_model::RevoluteJointModel* copied_revolute_joint_model = dynamic_cast<const robot_model::RevoluteJointModel*>(copied_joint_model);  
    const Eigen::Vector3d & joint_axis = copied_revolute_joint_model->getAxis();
    printf("\nr_forearm_roll_joint !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
    std::cout << joint_axis << std::endl;
    printf("\naxis vector !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");

So when I changed the arm position of the robot, I still get the same joint axis vector for a given joint. Following is the values that I got for different joints of pr2 and those values cannot change when the robot state changes:

Joint 1: (0,0,1)
Joint 2: (0,1,0)
Joint 3: (1,0,0)
Joint 4: (0,1,0)
Joint 5: (1,0,0)
Joint 6: (0,1,0)
Joint 7: (1,0,0)

Those axis vectors seems like the axis vectors for default arm position of pr2. I don't know why I keep getting same vectors for each joint when robot states changes.

Thanks for any help in advance!