0
$\begingroup$

Rosanswers logo

Hi, I am trying to control my 5 DoF arm. Using demo.launch from the setup assitant, dragging the end effector in rviz then planning works but when I try to control the end effector using cpp code, it fails to plan even though the target pose is the same or achievable via drag and drop. Here is the code I am using from the moveit tutorial

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

  // ROS spinning must be running for the MoveGroupInterface to get information
  // about the robot's state. One way to do this is to start an AsyncSpinner
  // beforehand.
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // Setup
  // ^^^^^
  // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
  // are used interchangeably.
  static const std::string PLANNING_GROUP = "arm";

  // The :planning_interface:`MoveGroupInterface` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);

  // We will use the :planning_interface:`PlanningSceneInterface`
  // class to add and remove collision objects in our "virtual world" scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  const moveit::core::JointModelGroup* joint_model_group =
      move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  //
  // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
  visual_tools.deleteAllMarkers();
  visual_tools.loadRemoteControl();
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.0;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  ROS_INFO_NAMED("Moveit", "Planning frame: %s", move_group_interface.getPlanningFrame().c_str());
  ROS_INFO_NAMED("Moveit", "End effector link: %s", move_group_interface.getEndEffectorLink().c_str());
  ROS_INFO_NAMED("Moveit", "Available Planning Groups:");
  std::copy(move_group_interface.getJointModelGroupNames().begin(),
            move_group_interface.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));
  std::cout<<std::endl;
  

  ROS_INFO_NAMED("Moveit", "Current Pose (x): %f", move_group_interface.getCurrentPose().pose.position.x);
  ROS_INFO_NAMED("Moveit", "Current Pose (y): %f", move_group_interface.getCurrentPose().pose.position.y);
  ROS_INFO_NAMED("Moveit", "Current Pose (z): %f", move_group_interface.getCurrentPose().pose.position.z);
  ROS_INFO_NAMED("Moveit", "Current Pose (Orientation x): %f", move_group_interface.getCurrentPose().pose.orientation.x);
  ROS_INFO_NAMED("Moveit", "Current Pose (Orientation y): %f", move_group_interface.getCurrentPose().pose.orientation.y);
  ROS_INFO_NAMED("Moveit", "Current Pose (Orientation z): %f", move_group_interface.getCurrentPose().pose.orientation.z);
  ROS_INFO_NAMED("Moveit", "Current Pose (Orientation w): %f", move_group_interface.getCurrentPose().pose.orientation.w);

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 0.619100;  
  target_pose1.orientation.x = 0.619101;  
  target_pose1.orientation.y = -0.341638;  
  target_pose1.orientation.z = 0.341634;  
  target_pose1.position.x = 0.035867;
  target_pose1.position.y = -0.002850;
  target_pose1.position.z = 0.300663;
  move_group_interface.setPoseTarget(target_pose1);

  // Now, we call the planner to compute the plan and visualize it.
  // Note that we are just planning, not asking move_group_interface
  // to actually move the robot.
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  ros::shutdown();
  return 0;
}

srdf file:

<robot name="arm">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="arm_eef">
        <link name="link_1"/>
        <link name="link_2"/>
        <link name="link_3"/>
        <link name="link_4"/>
        <link name="link_5"/>
        <link name="virtual_eef"/>
        <joint name="joint_1"/>
        <joint name="joint_2"/>
        <joint name="joint_3"/>
        <joint name="joint_4"/>
        <joint name="joint_5"/>
        <joint name="virtual_eef_joint"/>
        <chain base_link="base_link" tip_link="virtual_eef"/>
    </group>
    <group name="eef">
        <link name="eef_link_1"/>
        <link name="eef_finger_link_1"/>
        <link name="eef_link_2"/>
        <link name="eef_finger_link_2"/>
        <joint name="eef_joint_1"/>
        <joint name="eef_finger_joint_1"/>
        <joint name="eef_joint_2"/>
        <joint name="eef_finger_joint_2"/>
    </group>
    <group name="arm">
        <link name="link_1"/>
        <link name="link_2"/>
        <link name="link_3"/>
        <link name="link_4"/>
        <link name="link_5"/>
        <joint name="joint_1"/>
        <joint name="joint_2"/>
        <joint name="joint_3"/>
        <joint name="joint_4"/>
        <joint name="joint_5"/>
        <chain base_link="base_link" tip_link="link_5"/>
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="home" group="arm_eef">
        <joint name="joint_1" value="0"/>
        <joint name="joint_2" value="0"/>
        <joint name="joint_3" value="0"/>
        <joint name="joint_4" value="0"/>
        <joint name="joint_5" value="0"/>
    </group_state>
    <group_state name="home" group="arm">
        <joint name="joint_1" value="0"/>
        <joint name="joint_2" value="0"/>
        <joint name="joint_3" value="0"/>
        <joint name="joint_4" value="0"/>
        <joint name="joint_5" value="0"/>
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="eef" parent_link="virtual_eef" group="eef"/>
    <end_effector name="eef" parent_link="link_5" group="eef"/>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="base_link_1_joint" type="fixed" parent_frame="base_link" child_link="link_1"/>
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="eef_link_1" reason="Never"/>
    <disable_collisions link1="base_link" link2="eef_link_2" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_2" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_3" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_4" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_5" reason="Never"/>
    <disable_collisions link1="eef_finger_link_1" link2="eef_link_1" reason="Adjacent"/>
    <disable_collisions link1="eef_finger_link_1" link2="eef_link_2" reason="Never"/>
    <disable_collisions link1="eef_finger_link_1" link2="link_1" reason="Never"/>
    <disable_collisions link1="eef_finger_link_1" link2="link_2" reason="Never"/>
    <disable_collisions link1="eef_finger_link_1" link2="link_3" reason="Never"/>
    <disable_collisions link1="eef_finger_link_1" link2="link_4" reason="Never"/>
    <disable_collisions link1="eef_finger_link_2" link2="eef_link_2" reason="Adjacent"/>
    <disable_collisions link1="eef_finger_link_2" link2="link_1" reason="Never"/>
    <disable_collisions link1="eef_finger_link_2" link2="link_2" reason="Never"/>
    <disable_collisions link1="eef_finger_link_2" link2="link_3" reason="Never"/>
    <disable_collisions link1="eef_finger_link_2" link2="link_4" reason="Never"/>
    <disable_collisions link1="eef_link_1" link2="eef_link_2" reason="Adjacent"/>
    <disable_collisions link1="eef_link_1" link2="link_1" reason="Never"/>
    <disable_collisions link1="eef_link_1" link2="link_2" reason="Never"/>
    <disable_collisions link1="eef_link_1" link2="link_3" reason="Never"/>
    <disable_collisions link1="eef_link_1" link2="link_5" reason="Adjacent"/>
    <disable_collisions link1="eef_link_2" link2="link_1" reason="Never"/>
    <disable_collisions link1="eef_link_2" link2="link_2" reason="Never"/>
    <disable_collisions link1="eef_link_2" link2="link_3" reason="Never"/>
    <disable_collisions link1="eef_link_2" link2="link_4" reason="Never"/>
    <disable_collisions link1="eef_link_2" link2="link_5" reason="Adjacent"/>
    <disable_collisions link1="link_1" link2="link_2" reason="Adjacent"/>
    <disable_collisions link1="link_1" link2="link_3" reason="Never"/>
    <disable_collisions link1="link_1" link2="link_4" reason="Never"/>
    <disable_collisions link1="link_1" link2="link_5" reason="Never"/>
    <disable_collisions link1="link_2" link2="link_3" reason="Adjacent"/>
    <disable_collisions link1="link_2" link2="link_4" reason="Never"/>
    <disable_collisions link1="link_2" link2="link_5" reason="Never"/>
    <disable_collisions link1="link_3" link2="link_4" reason="Adjacent"/>
    <disable_collisions link1="link_3" link2="link_5" reason="Never"/>
    <disable_collisions link1="link_4" link2="link_5" reason="Adjacent"/>
</robot>

output of rviz terminal:

[ INFO] [1650883326.771500985]: Using position only ik
[ INFO] [1650883326.774897466]: Using position only ik
[ INFO] [1650883326.833175548]: Starting planning scene monitor
[ INFO] [1650883326.834660965]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1650883326.861750665]: Constructing new MoveGroup connection for group 'arm' in namespace ''
[ INFO] [1650883327.944611518]: Ready to take commands for planning group arm.
[ INFO] [1650883336.207896358]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1650883336.209582459]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1650883336.209981847]: arm/arm: Starting planning with 1 states already in datastructure
[ERROR] [1650883336.340840778]: arm/arm: Unable to sample any valid states for goal tree
[ INFO] [1650883336.340872099]: arm/arm: Created 1 states (1 start + 0 goal)
[ INFO] [1650883336.340896009]: No solution found after 0.130989 seconds
[ INFO] [1650883336.340937680]: Unable to solve the planning problem

output of moveitcpp terminal:

[ INFO] [1650883333.279265294]: Loading robot model 'arm'...
[ WARN] [1650883333.279633391]: Skipping virtual joint 'base_link_1_joint' because its child frame 'link_1' does not match the URDF frame 'base_link'
[ INFO] [1650883333.279646581]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1650883333.317826972]: Using position only ik
[ INFO] [1650883333.320478470]: Using position only ik
[ INFO] [1650883334.555488461]: Ready to take commands for planning group arm.
[ INFO] [1650883334.910042500]: RemoteControl Ready.
[ WARN] [1650883335.410632247]: Topic '/rviz_visual_tools' unable to connect to any subscribers within 0.5 sec. It is possible initially published visual messages will be lost.
[ INFO] [1650883335.411032454]: Planning frame: base_link
[ INFO] [1650883335.411060985]: End effector link: link_5
[ INFO] [1650883335.411075535]: Available Planning Groups:
arm, arm_eef, eef, 
[ INFO] [1650883335.508039080]: Current Pose (x): 0.035867
[ INFO] [1650883335.607931891]: Current Pose (y): -0.002850
[ INFO] [1650883335.707951203]: Current Pose (z): 0.300663
[ INFO] [1650883335.808033425]: Current Pose (Orientation x): 0.619101
[ INFO] [1650883335.908057476]: Current Pose (Orientation y): -0.341638
[ INFO] [1650883336.007838783]: Current Pose (Orientation z): 0.341634
[ INFO] [1650883336.108068369]: Current Pose (Orientation w): 0.619100
[ WARN] [1650883336.341255126]: Fail: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1650883336.341296947]: Visualizing plan 1 (pose goal) FAILED

Originally posted by MrOCW on ROS Answers with karma: 58 on 2022-04-25

Post score: 0


Original comments

Comment by crnewton on 2022-04-25:
Try some different planning parameters: other planner, more attempts , more time, Also does controlling using joint angles work? (i'm not sure if moving the endeffector in rviz uses IK or joint angles). I had similiar behaviour as your problem, for me all above didn't work, it turned out something was wrong with my MoveIt configuration, despite no error in check_urdf or the moveit_setup_assistant. ( I ended up creating a new MoveIt _config package)

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Solution is to use setApproximateJointValueTarget instead of setPoseTarget if use_position_only is not set to True. This is because the arm does not have >6DoF hence, we need to use Approximate IK Solution by using setApproximateJointValueTarget. However, this tends to provide far from decent solutions sometimes.

Use setPositionTarget if use_position_only: True


Originally posted by MrOCW with karma: 58 on 2022-04-25

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.