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

asked 2013-09-05 17:33:02 -0500

AdrianPeng gravatar image

updated 2013-11-14 10:41:52 -0500

tfoote gravatar image

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;

    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!

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2017-12-05 18:40:18.445677