I have two DC Servo motors with step and direction pins. But i dont have any feedback from the motor. I had read earlier that moveIt requires closed loop feedback control for operation.
[Specifications of motor](http://www.robokits.co.in/documentation/RMCS225x_DCServo_Driver.pdf)
↧
Can i use MOveIt package in ROS without closed loop motor control?
↧
how to set orientation constraints to my end effector?
Hi all,
I would like my UR5's end-effector to have an orientation constraint. Here is the code I used to try to achieve that:
//Adding Path Constraints
planning_interface::MotionPlanRequest req;
std::vector tolerance_pose(3, 0.01);
std::vector tolerance_angle(3, 0.01);
geometry_msgs::PoseStamped pose;
moveit_msgs::Constraints target_pose =
kinematic_constraints::constructGoalConstraints("wrist_3_joint", pose, tolerance_pose, tolerance_angle);
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "ee_link"; //"wrist_3_link";
quaternion.quaternion.x = 0;
quaternion.quaternion.y = 0;
quaternion.quaternion.z = 0;
quaternion.quaternion.w = 1;
req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_joint", quaternion);
I am pretty sure there is something wrong in this code, because the end effector sometimes changes his orientation during the execution of the plan.
Any help?
ROS kinetic, ubuntu 16.04
↧
↧
How to see visualization of execution of motion on RVIZ using MoveIt
I want to see the trail/visualization of motion when i execute a motion. Both these options(Trail/Animation) are switched on in the rviz motion planning menu. What can be done? Does this mean that there is some fault with the urdf or moveit_config file?
The motion planning trajectory slider also doesn't work.
[Picture of Arm on Rviz](https://drive.google.com/file/d/1u81NDZN1wOF4gxA8IGcugao7jVGrJUNg/view?usp=sharing)
↧
Controlling action server with Moveit
Hi,
I'm attempting to build ROS, and specifically moveit, support for a 5 jointed robot arm that until now has had no ROS integration. I've made a moveit config file that I can load correctly into rviz and plan paths with, using the demo.launch file, but when I attempt to plan and execute, my follow_joint_trajectory action server never receives anything. Right now the action server isn't connected to the robot itself and I'm just trying to check it can receive a trajectory at all. Here is my code for the server, however it initialises correctly and I'm pretty sure this isn't the problem.
#! /usr/bin/env python
import rospy
import actionlib
from control_msgs.msg import (
FollowJointTrajectoryAction,
FollowJointTrajectoryFeedback,
FollowJointTrajectoryResult,
)
from trajectory_msgs.msg import (
JointTrajectoryPoint
)
class JointTrajectoryActionServer(object):
def __init__(self, controller_name):
self._action_ns = controller_name + '/follow_joint_trajectory'
self._as = actionlib.SimpleActionServer(
self._action_ns,
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start = False)
self._action_name = rospy.get_name()
self._as.start()
self._feedback = FollowJointTrajectoryFeedback
self._result = FollowJointTrajectoryResult
rospy.loginfo('Successful init')
def execute_cb(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
rospy.loginfo(trajectory_points)
if __name__ == '__main__':
rospy.init_node('r12_interface')
server = JointTrajectoryActionServer('r12_arm_controller')
rospy.spin()
I also followed the moveit tutorials to make a controllers.yaml:
controller_list:
- name: r12_arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints: - waist_joint
- shoulder_joint
- elbow_joint
- hand_joint
- wrist_joint
and a moveit controller manager launch.xml
↧
MoveIt plan fails after attaching an object, but not from Rviz or after restarting requesting node
I am implementing a tool change procedure by attaching a collision object to the robot's gripper. However, after attaching the object, the planner fails to sample any valid states for the goal tree. More strangely, if I leave the scene untouched, restart the node without re-attaching the object, and execute the same planning requests, they succeed. Launching the planning requests from Rviz works, and goToNamedPose (see code below) also works, strangely.
I figure that means that the move group itself is intact, but something pollutes my code that causes the planning to fail. What is it? What could I do to debug further? Setting the rqt_logger_level to debug did not turn up any useful messages from the move group.
I have tried to condense the code to the pertinent parts. I can send a dockerized sandbox to anyone who is interested in checking this out, it just contains other messy code that I wouldn't want to upload needlessly.
Any pointers or ideas are appreciated.
main()
{
// ROS_INFO("Testing the tool mounting.");
o2as_skill_server.equipTool("b_bot", "tool_m5");
o2as_skill_server.goToNamedPose("home_b", "b_bot");
geometry_msgs::PoseStamped target_ps = makePoseStamped();
target_ps.header.frame_id = "tray_1";
target_ps.pose.position.z = .02;
// We ignore the orientation of the (they always point down) and assign an appropriate orientation
// TODO: How to find a good pose for the robot automatically? Launch a planning request to MoveIt?
target_ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -(110.0/180.0 *M_PI)); // Z-axis pointing up.
std::string link_name = "b_bot_tool_m5_tip_link";
std::string robot_name = "b_bot";
moveToCartPosePTP(target_ps, robot_name, true, link_name);
o2as_skill_server.unequipTool("b_bot");
ROS_INFO("Done.");
}
bool equipTool(std::string robot_name, std::string tool_id) {return equipUnequipTool(robot_name, tool_id, "equip");}
bool unequipTool(std::string robot_name) {return equipUnequipTool(robot_name, held_tool_, "unequip");}
bool equipUnequipTool(std::string robot_name, std::string tool_id, std::string equip_or_unequip)
{
bool equip = (equip_or_unequip == "equip");
bool unequip = (equip_or_unequip == "unequip");
// Plan & execute motion to in front of tool holder
geometry_msgs::PoseStamped ps_approach, ps_tool_holder;
ps_approach.header.frame_id = tool_id + "_link";
ps_approach.pose.position.y = .1;
ps_approach.pose.position.z = .017;
ps_approach.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -M_PI / 2);
ROS_INFO("Moving to tool approach pose PTP.");
moveToCartPosePTP(ps_approach, robot_name);
if (equip) {
openGripper(robot_name);
ROS_INFO("Spawning tool. Waiting for a while to be safe.");
spawnTool(tool_id);
}
// Plan & execute linear motion to the tool change position
ps_tool_holder = ps_approach;
ps_tool_holder.pose.position.y = -0.03;
ROS_INFO("Moving to pose in tool holder LIN.");
moveToCartPoseLIN(ps_tool_holder, robot_name);
// Close gripper, attach the tool object to the gripper in the Planning Scene.
if (equip)
{
closeGripper(robot_name);
attachTool(tool_id, robot_name);
}
else if (unequip)
{
openGripper(robot_name);
detachTool(tool_id, robot_name);
}
// Plan & execute linear motion away from the tool change position
ROS_INFO("Moving back to tool approach pose LIN.");
moveToCartPoseLIN(ps_approach, robot_name);
if (unequip) despawnTool(tool_id);
return true;
}
bool moveToCartPosePTP(geometry_msgs::PoseStamped pose, std::string robot_name, bool wait, std::string end_effector_link)
{
moveit::planning_interface::MoveGroupInterface::Plan myplan;
moveit::planning_interface::MoveItErrorCode
success_plan = moveit_msgs::MoveItErrorCodes::FAILURE,
motion_done = moveit_msgs::MoveItErrorCodes::FAILURE;
moveit::planning_interface::MoveGroupInterface* group_pointer;
group_pointer = robotNameToMoveGroup(robot_name);
group_pointer->setStartStateToCurrentState();
group_pointer->setEndEffectorLink(end_effector_link);
group_pointer->setPoseTarget(pose);
ROS_DEBUG_STREAM("Planning motion for robot " << robot_name << " and EE link " << end_effector_link + "_tip_link.");
success_plan = group_pointer->move(myplan);
if (success_plan)
{
if (wait) motion_done = group_pointer->execute(myplan);
else motion_done = group_pointer->asyncExecute(myplan);
if (motion_done) {
return true;
}
}
ROS_WARN("Failed to perform motion.");
return false;
}
bool goToNamedPose(std::string pose_name, std::string robot_name)
{
moveit::planning_interface::MoveGroupInterface* group_pointer;
group_pointer = robotNameToMoveGroup(robot_name);
group_pointer->setStartStateToCurrentState();
group_pointer->setNamedTarget(pose_name);
moveit::planning_interface::MoveGroupInterface::Plan myplan;
moveit::planning_interface::MoveItErrorCode
success_plan = moveit_msgs::MoveItErrorCodes::FAILURE,
motion_done = moveit_msgs::MoveItErrorCodes::FAILURE;
success_plan = group_pointer->move();
return true;
}
bool spawnTool(std::string tool_id)
{
std::vector collision_objects;
collision_objects.resize(1);
moveit_msgs::CollisionObject tool_m5;
tool_m5.header.frame_id = "tool_m5_link";
tool_m5.id = "tool_m5";
tool_m5.operation = tool_m5.ADD;
tool_m5.primitives.resize(1);
tool_m5.primitive_poses.resize(1);
tool_m5.primitives[0].type = tool_m5.primitives[0].BOX;
tool_m5.primitives[0].dimensions.resize(3);
tool_m5.primitives[0].dimensions[0] = 0.026;
tool_m5.primitives[0].dimensions[1] = 0.04;
tool_m5.primitives[0].dimensions[2] = 0.055;
tool_m5.primitive_poses[0].position.x = 0;
tool_m5.primitive_poses[0].position.y = -0.009;
tool_m5.primitive_poses[0].position.z = 0.0275;
if (tool_id == "tool_m5") collision_objects[0] = tool_m5;
collision_objects[0].operation = collision_objects[0].ADD;
planning_scene_interface_.applyCollisionObjects(collision_objects);
return true;
}
// Remove the tool from the scene so it does not cause unnecessary collision calculations
bool despawnTool(std::string tool_id)
{
std::vector collision_objects;
collision_objects.resize(1);
collision_objects[0].id = tool_id;
collision_objects[0].operation = collision_objects[0].REMOVE;
planning_scene_interface_.applyCollisionObjects(collision_objects);
return true;
}
bool attachDetachTool(std::string tool_id, std::string robot_name, std::string attach_or_detach)
{
moveit_msgs::AttachedCollisionObject att_coll_object;
if (tool_id == "tool_m5") att_coll_object.object = tool_m5;
else { ROS_WARN_STREAM("No tool specified to " << attach_or_detach); }
att_coll_object.link_name = robot_name + "_gripper_tip_link";
if (attach_or_detach == "attach") att_coll_object.object.operation = att_coll_object.object.ADD;
else if (attach_or_detach == "detach") att_coll_object.object.operation = att_coll_object.object.REMOVE;
ROS_INFO_STREAM(attach_or_detach << "ing tool " << tool_id);
planning_scene_interface_.applyAttachedCollisionObject(att_coll_object);
return true;
}
↧
↧
Moveit & singularities
Hi all,
I was wondering if MoveIt actually knows about robot singularities... because when I execute trajectories sometimes the robot makes a little oscillation. It seems that it stops for a little time and then restart moving. Is this because of the robot is passing close a singularity configuration? Or is there another reason?
Ros Kinetic, Ubuntu 16.04
↧
How to send MoveIt joint control data to actual motor
I want to the motor of my robotic arm based on effort/velocity data that i get after planning trajectory on move it. How do i connect my motor and get it to move?
Motor Specification: Rhino DC-servo with step/direction drive
↧
Details of MoveIt! to OMPL Pipeline
I have recently been trying to wrap my head around the pipeline between MoveIt! and OMPL.
Currently, my main issue is that I want to be able to take a state generated by OMPL and map it back to what it means in the context of the planning problem. This help me answer important questions such as "what configurations is the planner sampling?" and "why does one planner perform better/worse than the other?".
As stated in the [documentation](http://moveit.ros.org/documentation/planners/), "planners in OMPL are abstract; i.e. OMPL has no concept of a robot. Instead, MoveIt! configures OMPL and provides the back-end for OMPL to work with problems in Robotics."
From reading through the OMPL documentation, I understand that OMPL [requires at least a StateSpace, StateValidityChecker, and a ControlSpace](http://ompl.kavrakilab.org/api_overview.html) used in conjunction with the SimpleSetup object to plan.
On the other hand, it seems that MoveIt! generates a motion plan by sending a MotionPlanRequest through a pipeline that converts the problem into a form understandable to the planner. This pipeline is what I am interested in.
My questions are:
1. How does MoveIt convert a Motion Plan Request to a StateSpace, StateValidityChecker, and Control Space that OMPL needs?
2. Given that OMPL has no sense of what a robot is, how does MoveIt! abstract the problem down to a form solvable by OMPL?
3. Is there a way to gain some physical intuition on what the states explored by the planner are? Or are the intermediate plans stuck in a black box with only the solution reported by the planning plugin?
Some approaches I have taken are:
1. Look through the [documentation](http://moveit.ros.org/documentation/concepts/) and the [tutorial](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ompl_interface/ompl_interface_tutorial.html) on MoveIt! and OMPL. But it seems to be more focused on the high level implementation rather than how the interface works/
2. Look at the [source code](https://github.com/ros-planning/moveit/tree/kinetic-devel/moveit_planners/ompl) for the OMPL interface in Moveit!. The code is relatively complex and difficult to navigate, specifically, it is difficult to tell how the adapter plugins and the classes interact. Some guidance in understanding the code structure would also be greatly appreciated.
Thanks for your help!
↧
Robot link and Robot attached collision in MoveIt!
I'm trying to grasp an object on a table, attach it to the hand, and drop it off somewhere else with the Jaco2 robotic arm.
Grasping works fine: I attach the object and close the grippers, which leads me to believe the collision is (correctly!) ignored for gripping the object, but as soon as I want to drop it, I often get the following error.
[INFO] Found a contact between 'cup' [Robot attached] and 'jaco_finger_tip_1' [Robot link]
[WARN] Start State seems to be in collision with respect to group arm.
This happens in 1 out of 5 times, I would say. The Planner I use is RTTConnectkDefaultConfig.
I am sure, that the object is attached, for multiple reasons:
a. I have a fixed tabletop already in the planning scene. If i don't lower it a cm or so lower than it actually is, I get this error:
` [INFO] Found a contact between 'cup' [Robot attached] and 'jaco_finger_tip_1' [Robot link] `
b. I check again and again with the move_group interface, if object exists and if it is attached. I get no error messages here
auto all_objects = planning_scene_interface_.getObjects();
if (all_objects.find(box.description) == all_objects.end()) {
ROS_ERROR_STREAM("Object " << box.description << " hasn't been created");
return;
}
// moveit_visuals_.attachObstacle(box);
auto all_attached_objects = planning_scene_interface_.getAttachedObjects();
if (all_attached_objects.find(box.description) != all_attached_objects.end()) {
ROS_WARN_STREAM("Object " << box.description << " is already attached");
return;
}
ROS_STATUS("Attaching obstacle " << box.description);
move_group_.attachObject(box.description);
Adding objects to the planning scene works fine:
const auto& pose_goal = goal->pose_goal;
const auto& box = goal->bounding_box;
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_.getPlanningFrame();
collision_object.id = box.description;
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = box.dimensions.x;
primitive.dimensions[1] = box.dimensions.y;
primitive.dimensions[2] = box.dimensions.z;
geometry_msgs::PointStamped out_pt;
geometry_msgs::PointStamped in_pt;
in_pt.header = box.header;
in_pt.point = box.point;
// transform point
try {
tf_listener_.waitForTransform("root", box.header.frame_id, box.header.stamp, ros::Duration(1));
tf_listener_.transformPoint("root", in_pt, out_pt);
}
catch (tf::TransformException &exception) {
ROS_INFO_STREAM("Transform failed. Why? - " << exception.what());
}
geometry_msgs::Pose pose;
pose.position.x = out_pt.point.x;
pose.position.y = out_pt.point.y;
pose.position.z = out_pt.point.z;
pose.orientation.w = 1.0;
collision_object.operation = collision_object.ADD;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(pose);
std::vector collision_objects;
collision_objects.push_back(collision_object);
planning_scene_interface_.addCollisionObjects(collision_objects);
ROS_STATUS("Added Object " << box.description);
detaching/attaching in accordance with tutorials
void JacoManipulationServer::attachObstacle(const jaco_manipulation::BoundingBox &box) {
ROS_STATUS("Attaching obstacle " << box.description);
move_group_.attachObject(box.description);
}
void JacoManipulationServer::detachObstacle(const jaco_manipulation::BoundingBox &box) {
ROS_STATUS("Detaching obstacle " << box.description);
move_group_.detachObject(box.description);
}
this is my gripping code
addObstacle(goal);
bool moved = planAndMove(goal->pose_goal);
if (!moved) return false;
attachObstacle(goal->bounding_box);
closeGripper();
ROS_STATUS("Gripper closed. Object grasped.");
return true;
where i add the obstacle (code above), let moveit do planning and moeving towards goal, attach the obstacle, so it can be gripped, and close the gripper.
Similarly, for dropping
bool moved = planAndMove(goal->pose_goal);
if (!moved) {
openGripper();
detachObstacle(goal->bounding_box);
return false;
}
openGripper();
detachObstacle(goal->bounding_box);
ROS_STATUS("Gripper opened. Object dropped.");
return true;
I plan and move with moveit, open the grippers (and then detach the object, not the other way around, so opening the gripper on the detached object doesn't show collision)
Thanks a lot for the help
↧
↧
Obstacle information to MoveIt !!
Hello guys
I am using Ardrone and i am also successfully able to plan trajectory using Moveit. Now I have a obstacle detection ROS node which is giving the coordinates of the obstacle in front.
My doubt is how to actually feed this co-ordinate information to Moveit (ardrone), so as to get a obstacle free path. I know that i can actually use point cloud but i don't want to use that, I just want to publish those obstacle coordinates onto a topic to feed Moveit with that information.
I am using /ardrone/camera/image_raw topic.
Any suggestions !!
Thanks all.
↧
PRM roadmap building not working through movegroup
Hello,
I am using OMPL with ROS Kinetic and trying to make PRM work.
I have the following in ompl_planning.yaml
PRMkConfigDefault:
type: geometric::PRM
max_nearest_neighbors: 10
...
manipulator:
planner_configs:
- PRMkConfigDefault
...
endeffector:
planner_configs:
- PRMkConfigDefault
I set up and use the planner like this:
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit::planning_interface::MoveGroupInterface group;
group.setPlannerId("PRMkConfigDefault");
group.setStartState(start_state);
group.setPoseTarget(pose_target);
group.plan(my_plan);
The planning works, but the planner seems to start building the roadmap all over again for each request, as I see from this MoveGroup output:
[ INFO] [1534421007.766392320, 27.556000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1534421007.767078520, 27.556000000]: Planner configuration 'manipulator[PRMkConfigDefault]' will use planner 'geometric::PRM'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1534421007.767221225, 27.556000000]: manipulator[PRMkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1534421007.788053174, 27.574000000]: manipulator[PRMkConfigDefault]: Starting planning with 2 states already in datastructure
[ INFO] [1534421008.772725520, 28.196000000]: manipulator[PRMkConfigDefault]: Created 102 states
[ INFO] [1534421008.772951172, 28.196000000]: Solution found in 1.005560 seconds
[ INFO] [1534421008.773194629, 28.196000000]: SimpleSetup: Path simplification took 0.000100 seconds and changed from 2 to 2 states
[ INFO] [1534421008.800537690, 28.216000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1534421008.801144538, 28.216000000]: Planner configuration 'manipulator[PRMkConfigDefault]' will use planner 'geometric::PRM'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1534421008.801274953, 28.216000000]: manipulator[PRMkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1534421008.822160900, 28.229000000]: manipulator[PRMkConfigDefault]: Starting planning with 2 states already in datastructure
[ INFO] [1534421009.803937922, 28.867000000]: manipulator[PRMkConfigDefault]: Created 129 states
[ INFO] [1534421009.804883451, 28.869000000]: Solution found in 1.002696 seconds
[ INFO] [1534421009.805021586, 28.869000000]: SimpleSetup: Path simplification took 0.000005 seconds and changed from 2 to 2 states
as there should clearly be more than "2 states already in datastructure" during the second run.
How to do it, so that the planner stores already sampled states?
Thank you!
↧
unable to move arm with moveit!
On kinetic, Ubuntu 16.04, trying to make the arm of my robot move a little bit.
the code:
bool run_script(){
moveit::planning_interface::MoveGroupInterface move_group("arm");
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.7;
target_pose1.position.z = 1.0;
move_group.setPoseTarget(target_pose1);
move_group.move();
return true;
}
The log:
[ INFO] [1515508915.187395653, 2305.704000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1515508915.187550137, 2305.704000000]: Planning attempt 1 of at most 1
Debug: Starting goal sampling thread
[ INFO] [1515508915.188247424, 2305.704000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
Debug: The value of parameter 'longest_valid_segment_fraction' is now: '0.0050000000000000001'
Debug: The value of parameter 'range' is now: '0'
Debug: RRTConnect: Planner range detected to be 4.610440
Info: RRTConnect: Starting planning with 1 states already in datastructure
Debug: RRTConnect: Waiting for goal region samples ...
Debug: Beginning sampling thread computation
Error: RRTConnect: Unable to sample any valid states for goal tree
at line 215 in /tmp/binarydeb/ros-kinetic-ompl-1.2.1/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
Info: RRTConnect: Created 1 states (1 start + 0 goal)
Info: No solution found after 5.007760 seconds
Debug: Attempting to stop goal sampling thread...
Debug: Stopped goal sampling thread after 0 sampling attempts
[ INFO] [1515508920.215263508, 2309.500000000]: Unable to solve the planning problem
I even tried getting the current pose and change it a little:
bool run_script(){
moveit::planning_interface::MoveGroupInterface move_group("arm");
geometry_msgs::PoseStamped pose1 = move_group.getCurrentPose();
pose1.pose.position.x = pose1.pose.position.x + pose1.pose.position.x/4;
move_group.setPoseTarget(pose1);
move_group.move();
return true;
}
If you need anymore info let me know.
↧
MoveGroupCommander get_current_pose() returns incorrect result when using real robot
I want to perform a movement relative to the current pose of a UR robot, which I have prototyped in Python. When I am running the simulation and the physical robot, the same .urdf and .srdf files are loaded, so the scene known to MoveIt should be the same. However, when the real robot is connected, the output of the MoveGroupCommander function `get_current_pose()` is different from the simulation:
The code:
# ==== MISBEHAVING VERSION:
start_pos_mgc = group.get_current_pose()
# ==== WORKING VERSION:
gripper_pos = geometry_msgs.msg.PoseStamped()
gripper_pos.header.frame_id = "a_bot_gripper_tip_link"
gripper_pos.pose.orientation.w = 1.0
start_pos_manual = self.listener.transformPose("world", gripper_pos)
# ====
rospy.loginfo("The EE link is: " + group.get_end_effector_link())
rospy.loginfo("The Planning frame is: " + group.get_planning_frame())
rospy.loginfo("Pose via MoveGroupCommander is: ")
rospy.loginfo(start_pos_mgc)
rospy.loginfo("Pose via manual TF is: ")
rospy.loginfo(start_pos_manual)
When running the `demo.launch` of the moveit_config package, the two poses are the same, but when using the real robot, the output is this:
[INFO] [1534740471.283544]: The EE link is: a_bot_gripper_tip_link
[INFO] [1534740471.283821]: The Planning frame is: /world
[INFO] [1534740471.284032]: Pose via MoveGroupCommander is:
[INFO] [1534740471.284445]: header:
seq: 0
stamp:
secs: 1534740471
nsecs: 279306888
frame_id: "/world"
pose:
position:
x: 0.814750000001
y: -0.22355
z: 0.742509000003
orientation:
x: 0.499999999995
y: 0.5
z: 0.500000000002
w: 0.500000000002
[INFO] [1534740471.285085]: Pose via manual TF is:
[INFO] [1534740471.285504]: header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "world"
pose:
position:
x: -0.10676008707785575
y: -0.47442804315315057
z: 1.1395218025815932
orientation:
x: -0.7067673098798285
y: 0.007619800057650674
z: 0.7073708035668745
w: -0.006960933376064573
The pose returned by get_current_pose() does not change, even though the robot's position changes correctly and is displayed in rviz. The planning using rviz and in the rest of my code works fine, but I have not used get_current_pose() anywhere else before. I published markers for three calls to the code above (the three frames with green blobs near the robot are start_pos_manual, the one near the bottom of the screen is start_pos_mgc):
I cannot find what the issue is. I am especially confused that all of the other planning works, and . I tried digging in [the code](https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_commander/src/moveit_commander/move_group.py), but I'm not sure where `_moveit_move_group_interface` is imported from (if it's a wrapped C++ file then I might need a hand). What could be the issue?
I am using [this version of the ur_modern_driver](https://github.com/Zagitta/ur_modern_driver) on Kinetic, Ubuntu 16.04 and the binary version of MoveIt. Thanks in advance.
**edit:** I just noticed that start_pos_mgc seems to be where the end effector link would be with all of the robot joints at 0. There must be something going wrong with the joint states not being updated, or not being accessed correctly by get_current_pose(), but what could it be? I don't know why the planning works, but only get_current_pose() does not. For the record, the getCurrentPose() function in the C++ MoveGroupInterface class works fine.
↧
↧
How to get joint state data for a trajectory using MoveIt python interface?
I want to get the required arm joints rotation angles for a trajectory planned in moveIt. I am confused between ROS-Control, hardware_interface and actionlib.
↧
Moveit: adding objects to the scene using laser scanner
HI all,
I have an issue regarding the insertion of new objects to the scene. I am using a laser scanner for detecting obstacles, and when the variable obs_detection becomes true, I make a box appear in the environment. The problem is that if I don't place a delay before the appearance of the obstacle , the obstacle doesn't not appear at all in the scene. Same thing for the obstacle disappearance. Here is my code:
#include "obstacle_detector_ur5/obs_det.h"
obs_det::obs_det() {
obs_det::laser_sub_ = nh.subscribe("scan_raw",1,&obs_det::laserCB,this);
obs_detection = false;
rate = new ros::Rate(50);
visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("world","/moveit_visual_markers"));
//visual_tools = new moveit_visual_tools::MoveItVisualTools ("world");
}
obs_det::~obs_det()
{
}
void obs_det::spinner() {
ros::spinOnce();
this->refresh();
rate->sleep();
}
void obs_det::refresh() {
static const std::string PLANNING_GROUP = "manipulator";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("world");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.13;
primitive.dimensions[1] = 0.14;
primitive.dimensions[2] = 0.42;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.29; //old val 0.4
box_pose.position.y = 0.39; //old val -0.25
box_pose.position.z = 0.21; //old val 0.43
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector collision_objects;
collision_objects.push_back(collision_object);
if (obs_detection) {
ros::Duration(0.4).sleep(); //to make sure the node can publish
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
}
else {
ros::Duration(0.4).sleep();
ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
}
void obs_det::laserCB(const sensor_msgs::LaserScan::ConstPtr& laser) {
for (int i = 0; i < laser->ranges.size(); i++)
{
if (laser->ranges[i] < 0.20 && laser->ranges[i] > 0.05)
{
obs_detection = true;
return;
}
else
obs_detection = false;
}
}
Does anybody have an idea why this happens? Isn't there a way to add the object instantaneously?
Thanks for support
ROS Kinetic, Ubuntu 16.04
↧
What's next after loading controllers to move Arm?
I have cloned the github repository [safe_arm](https://github.com/byeongkyu/safe_arm.git)
On launching the relevant files the controllers are started. Now for example i have a robotic arm with the same configuration/design, how do i send the controllers commands to my actual dc motors via arduino?
Any help is appreciated.
↧
How to set up planner through PlannerManager
I am trying to use OMPL planners directly through Motion Planning API, as [described in this tutorial](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/motion_planning_api/motion_planning_api_tutorial.html) in Kinetic.
// RobotModelLoader
const std::string PLANNING_GROUP = "manipulator";
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
boost::scoped_ptr> planner_plugin_loader;
// Loading a planner instance
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name = "ompl_interface/OMPLPlanner";
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
planner_instance->initialize(robot_model, node_handle.getNamespace());
I would like to change and configure the planner used, which I attempt to achieve by setting it in PlannerManagerPtr planner_instance.
planning_interface::PlannerConfigurationSettings settings;
settings.name = "experimental";
settings.group = "manipulator";
settings.config.insert(pair("type", "geometric::RRTConnect"));
map settings_map;
settings_map.insert(pair(settings.name, settings));
planner_instance->setPlannerConfigurations(settings_map);
Then, I perform the planning request in PlanningContextPtr context. ID of the desired planner is a part of the planning request.
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base_link";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;
std::vector tolerance_pose(3, 0.01);
std::vector tolerance_angle(3, 0.01);
req.group_name = "manipulator";
req.allowed_planning_time = 1;
req.planner_id = "experimental";
moveit_msgs::Constraints pose_goal =
kinematic_constraints::constructGoalConstraints("ee_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
planning_interface::PlanningContextPtr context =
planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
However, I get the following warning
[ WARN] [1534843451.389670926, 38.268000000]: Cannot find planning configuration for group 'manipulator' using planner 'experimental'. Will use defaults instead.
and **the planner set in the planning request is not used**, which is the problem I want to solve.
When I do
planner_instance->getPlanningAlgorithms(algs);
my planner "experimental" is listed.
What am I doing wrong?
↧
↧
How to link FollowJointTrajectory action to move-group and gazebo?
I have modified the files of [safe_arm](https://github.com/byeongkyu/safe_arm)
Here is the node graph of safe arm generated using rqt followed by my own robots node graph
The follow joint trajectory action is not connected to gazebo but it shows up on doing rostopic list.
Can anybody tell what I am missing here?
↧
MoveIt: Error: Action client not connected
On running move it for my custom robotic arm i am getting the following error
[ERROR] [1534930610.891287885, 28.494000000]: Action client not connected: uvbot_v1/arm_joint_controller/follow_joint_trajectory
[ERROR] [1534930610.891287885, 28.494000000]: Action client not connected: uvbot_v1/arm_joint_controller/follow_joint_trajectory
Can anyone explain how to setup action client? The tutorials are a bit sketchy.
↧
moveit: param server is missing planner configurations
Hello all,
here are the errors,
[ERROR] [1528993777.322079817]: Could not find the planner configuration 'RRTConneckConfigDefault' on the param server
[ERROR] [1528993777.326028820]: Could not find the planner configuration 'SBLkConfigDefault' on the param server
[ERROR] [1528993777.326619022]: Could not find the planner configuration 'ESTkConfigDefault' on the param server
[ERROR] [1528993777.327209350]: Could not find the planner configuration 'LBKPIECEkConfigDefault' on the param server
[ERROR] [1528993777.327770784]: Could not find the planner configuration 'BKPIECEkConfigDefault' on the param server
[ERROR] [1528993777.328322494]: Could not find the planner configuration 'KPIECEkConfigDefault' on the param server
[ERROR] [1528993777.328897858]: Could not find the planner configuration 'RRTkConfigDefault' on the param server
[ERROR] [1528993777.329486582]: Could not find the planner configuration 'RRTConnectkConfigDefault' on the param server
[ERROR] [1528993777.330046872]: Could not find the planner configuration 'RRTstarkConfigDefault' on the param server
[ERROR] [1528993777.330620285]: Could not find the planner configuration 'TRRTkConfigDefault' on the param server
[ERROR] [1528993777.331185690]: Could not find the planner configuration 'PRMkConfigDefault' on the param server
[ERROR] [1528993777.331821514]: Could not find the planner configuration 'PRMstarkConfigDefault' on the param server
[ERROR] [1528993777.332365152]: Could not find the planner configuration 'FMTkConfigDefault' on the param server
[ERROR] [1528993777.332903057]: Could not find the planner configuration 'BFMTkConfigDefault' on the param server
[ERROR] [1528993777.333557242]: Could not find the planner configuration 'PDSTkConfigDefault' on the param server
[ERROR] [1528993777.334391734]: Could not find the planner configuration 'STRIDEkConfigDefault' on the param server
[ERROR] [1528993777.335209338]: Could not find the planner configuration 'BiTRRTkConfigDefault' on the param server
[ERROR] [1528993777.337834268]: Could not find the planner configuration 'LBTRRTkConfigDefault' on the param server
[ERROR] [1528993777.340810491]: Could not find the planner configuration 'BiESTkConfigDefault' on the param server
[ERROR] [1528993777.343505094]: Could not find the planner configuration 'ProjESTkConfigDefault' on the param server
[ERROR] [1528993777.344906710]: Could not find the planner configuration 'LazyPRMkConfigDefault' on the param server
[ERROR] [1528993777.346300595]: Could not find the planner configuration 'LazyPRMstarkConfigDefault' on the param server
[ERROR] [1528993777.347891216]: Could not find the planner configuration 'SPARSkConfigDefault' on the param server
[ERROR] [1528993777.352570257]: Could not find the planner configuration 'SPARStwokConfigDefault' on the param server
i know they are from the drop down menu of the OMPL planning library section. what should i edit to fix this?
EDIT:
Current ompl_planning.yaml (fixed through the fix i mentioned)
planner_configs:
SBL:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
EST:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECE:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECE:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
...
manipulator:
default_planner_config: RRTConnect
planner_configs:
- SBL
- EST
- LBKPIECE
- BKPIECE
- ...
projection_evaluator: joints(joint_1,joint_2)
longest_valid_segment_fraction: 0.005
Prior to the fix, all of the names under the "planner_configs:" line were concatenated with kConfigDefault, along with the base names up top such as TRRT, RRTConnect, etc. Moveit looks for the base names, and a default, but if all of them are labeled default like mine was, it caused an issue.
↧