Ask Your Question
1

Arm navigation with a virtual robot

asked 2011-10-13 03:16:16 -0500

updated 2011-10-13 03:21:56 -0500

I have read the tutorial on the new arm_navigation architecture, and I am really excited to try it out. As most tutorials have not been updated yet, do you have any code example that shows how to plan a movement using a robot position which is different from the current one?

Let's say the robot is at (x,y,th), and I want to see if I can reach a place by moving the robot to (x', y', th'). How can I do it using the planning scene?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2011-10-19 05:18:54 -0500

egiljones gravatar image

Lorenzo,

It should be pretty easy. Basically, you are going to put a different transform between the world frame (in this case 'odom_combined') and the frame associated with the base of the robot ('base_footprint'). If operating with a running system you'll take the current state, apply whatever multi-dof and joint states are in the planning_scene_diff, and push the result to all components, who will then respond accordingly. You'll want to interact with the components directly instead of using MoveArm, as you probably don't want to actually act based on state that's not the current one, though you are given the option.

Here's some code for generating IK solutions given another base configuration for your robot:

#include <ros/ros.h>

#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetConstraintAwarePositionIK.h>
#include <arm_navigation_msgs/SetPlanningSceneDiff.h>

static const std::string ARM_COLLISION_IK_NAME = "/pr2_right_arm_kinematics/get_constraint_aware_ik";
static const std::string ARM_QUERY_NAME = "/pr2_right_arm_kinematics/get_ik_solver_info";
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

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

  ros::service::waitForService(ARM_QUERY_NAME);
  ros::service::waitForService(ARM_COLLISION_IK_NAME);
  ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);

  ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME);
  ros::ServiceClient ik_with_collision_client = rh.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(ARM_COLLISION_IK_NAME);

  arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
  arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;

  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.stamp = ros::Time::now();
  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names.push_back("world_joint");
  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids.push_back("odom_combined");
  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids.push_back("base_footprint");
  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.poses.resize(1);
  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.poses[0].position.x = 4.0;
  planning_scene_req.planning_scene_diff.robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;

  if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
    ROS_WARN("Can't get planning scene");
    return -1;
  }

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;

  if(query_client.call(request,response))
  {
    for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
    }
  }
  else
  {
    ROS_ERROR("Could not call query service");
    ros::shutdown();
    exit(-1);
  }

  // define the service messages
  kinematics_msgs::GetConstraintAwarePositionIK::Request  gpik_req;
  kinematics_msgs::GetConstraintAwarePositionIK::Response gpik_res;

  gpik_req.timeout = ros::Duration(5.0);
  gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";

  gpik_req.ik_request.pose_stamped.header.frame_id = "odom_combined";
  gpik_req.ik_request.pose_stamped.pose.position.x = 4.75;
  gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
  gpik_req.ik_request.pose_stamped.pose.position.z = .94;

  gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
  gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;

  gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
  gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;

  for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
  {
    gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
  }
  if(ik_with_collision_client.call(gpik_req, gpik_res))
  {
    if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
    {
      for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
      {
        ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state ...
(more)
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2011-10-13 03:16:16 -0500

Seen: 328 times

Last updated: Oct 19 '11