welcome[bot]
commented
4 years ago Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.
Open rishabhgks opened 4 years ago
welcome[bot]
commented
4 years ago Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.
rishabhgks
commented
4 years ago @JafarAbdi @henningkayser @rhaschke Could any of you please guide me on where I might be going wrong here?
corot
commented
3 years ago I'm experiencing the same issue when picking/placing in gazebo, but only with the current melodic debians. Doesn't happen if I use compiled master (didn't try compiled melodic)
...
-
positions: [1.2889687027773171, 0.055803899290690184, -0.6248854557842535, 0.7660033740710387]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
-
positions: [1.2434171380344599, 0.15359409387309908, -0.6561297106314428, 0.7343035428209158]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
-
positions: [1.1978655732916028, 0.25138428845550775, -0.687373965478632, 0.7026037115707929]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
-
positions: [1.1523140085487455, 0.34917448303791687, -0.7186182203258215, 0.6709038803206699]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 0
...Funny thing is that this started happening after re-making my moveit configuration with an updated wizard! (here the update commit)
Description
I am trying to use the MoveItCpp interface to control a Staubli TX-60 arm. I use their JointTrajectoryController and set the "moveit_controller_manager" to MoveItSimpleControllerManager. The planning happens perfectly and a plan is generated, but when I run the execute() function I get the error:
I tried to debug further and came at the point where planning_component returns a solution to the moveit_cpp_tutorial. I converted the plan_solution1->trajectory to moveit_msgs/RobotTrajectory msg and printed it out on a topic. This is an example of the type of messages being passed by the planner to the TrajectoryExecutionManager:
So the planner is able to plan the multiple positions for the trajectory, but the velocities, accelerations and effort are all empty and more importantly the time_of_start for all of them is 0 which leads to the error that Trajectory Messages are not increasing in time. From here on I'm out of my depth to debug and find out how to resolve this issue.
Your environment
Steps to reproduce
Create a launch file and paste the following code inside it.
include <ros/ros.h>
include
// MoveitCpp // #include <moveit/moveit_cpp/moveit_cpp.h> // #include <moveit/moveit_cpp/planning_component.h>
include "../../moveit/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h"
include "../../moveit/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h"
include <geometry_msgs/PointStamped.h>
include <moveit_visual_tools/moveit_visual_tools.h>
namespace rvt = rviz_visual_tools;
int main(int argc, char** argv) { ros::init(argc, argv, "tx60_arm_moveitcpp"); ros::NodeHandle nh; ros::AsyncSpinner spinner(4); spinner.start();
// BEGIN_TUTORIAL // // Setup // ^^^^^ // static const std::string PLANNING_GROUP = "arm"; static const std::string LOGNAME = "tx60_arm_moveitcpp";
/ Otherwise robot with zeros joint_states / ros::Duration(1.0).sleep();
ROS_INFO_STREAM_NAMED(LOGNAME, "Starting TX60 arm control...");
auto moveit_cpp_ptr = std::make_shared(nh);
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
moveit_cpp_ptr->getPlanningSceneMonitorNonConst();
planning_scene_monitor->providePlanningSceneService("get_planning_scene");
auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr);
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
auto robot_start_state = planning_components->getStartState();
auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
// Visualization // ^^^^^^^^^^^^^ // // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script moveit_visual_tools::MoveItVisualTools visual_tools("base_link", rvt::RVIZ_MARKER_TOPIC, moveit_cpp_ptr->getPlanningSceneMonitor()); visual_tools.deleteAllMarkers(); visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.75; visual_tools.publishText(text_pose, "MoveItCpp Demo", rvt::WHITE, rvt::XLARGE); visual_tools.trigger();
// Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// Planning with MoveItCpp // ^^^^^^^^^^^^^^^^^^^^^^^ // There are multiple ways to set the start and the goal states of the plan // they are illustrated in the following plan examples // // Plan #1 // ^^^^^^^ // // We can set the start state of the plan to the current state of the robot planning_components->setStartStateToCurrentState();
// The first way to set the goal of the plan is by using geometry_msgs::PoseStamped ROS message type as follow geometry_msgs::PoseStamped target_pose1; target_pose1.header.frame_id = "base_link"; target_pose1.pose.orientation.w = 1.0; target_pose1.pose.position.x = 0.2; target_pose1.pose.position.y = 0.02; target_pose1.pose.position.z = 0.8; planning_components->setGoal(target_pose1, "tool0");
// Now, we call the PlanningComponents to compute the plan and visualize it. // Note that we are just planning auto plan_solution1 = planning_components->plan();
// Check if PlanningComponents succeeded in finding the plan if (plan_solution1) { // Visualize the start pose in Rviz visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("tool0"), "start_pose"); visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE); // Visualize the goal pose in Rviz visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); // Visualize the trajectory in Rviz visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); visual_tools.trigger();
}
// Start the next plan visual_tools.deleteAllMarkers(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// END_TUTORIAL visual_tools.deleteAllMarkers(); visual_tools.prompt("Press 'next' to end the demo");
ROS_INFO_STREAM_NAMED(LOGNAME, "Shutting down."); ros::waitForShutdown(); }