Hi everybody,
Situation I use the package arm_navigation with ros-electric to control a arm robot with seven degree of freedom. I can send a articular or Cartesian goal, the robot moves well and avoid the object from urdf file like the table where is the robot. I use the "robot_name"_arm_navigation.launch and a robot simulation which receive joint state y publish TF of urdf with the good position.
Problematic I would use the kinect to add man body in the scene and more particularly at the collision map for the robot can avoid the body. I want to actualize it everytime. I tried three solutions. I want add the body as several cylinders which moves in "real time".
Solution 1: I followed the tutorial Adding known objects to the collision environment to add cylinder from kinect information. In this code I just add in the topic collision_object 10 cylinders to modeling the body.
Results 1: I succeeded to add correctly all cylinder for each body part. For example a arm it's modeling by a cylinder. The robot avoids well the body but just at the beginning of his movement. During the trajectory cylinders doesn't move and I can't check collisions.
Question 1: How actualize the collision map for see cylinders moves ?
Solution 2: It's a similar solution with the tutorial Adding Virtual Objects to the Planning Scene. The difference it's only that I don't use a topic to add objects but a service. I don't see others differences with the solution 1.
Result 2: With this solution cylinders can move everytime. There isn't the same problem of solution 1. But There is a more important problem with it. This solution works only when I choose a coordinate system already present in the urdf and doesn't work with a coodinate system generated by Kinect like right_arm_1. I explain me, when I add a cylinder, I add in a coordinate system(TF) and in this case it's more easy to add cylinder directly in coordinate system of the body. And with right_arm_1 or openni_depth_frame as coordinate system the cylinders doesn't fix in this coordinate system but works perfectly with coordinate system created from urdf file. I added a link between a urdf coordinate system and openni_depth_frame but it's like if there isn't this link.
Question 2: Why the environment_server doesn't understand the new TF of Kinect system ? How fix this ?
Solution 3: We thought to add the body man directly in the urdf and modify directly the tf with a publication in /tf.
Result3: it doesn't work because the simulation publish every time all urdf tf by /robot_st_pub. Only we can modify joint state. And it's more complicated to calculate joint state of the boy.
Question 3: It's possible to use urdf to add and modify mobil object ?
thank you for your help. if I should add details ask me.
EDIT1: Hi, Thanks for your answer. I understand that the world is static during the planning and this it's good. I don't want add velocity information on my object.
My issue is when the robot follows his trajectory. During this movement I check the state validity of the 10 next positions to look if collision I have. For this, I use Checking a state for validity.
But this code only check with the world at the time of planning calculation and if I move the object to do a collision, isn't detected. That is my really issue, I hope you understand better.
Thanks
EDIT2:
my problem with solution2 is very strange with it. It works well when I write: cylinder_object.header.frame_id = "kinect_link"; kinect_link is a tf created in the urdf description. It works also with /world for example. But it doesn't work with a kinect frame like "openni_depth_frame" or "right_hand_1", "head", "torso" ect. The cylinder appears very far from the coordinate system. It's essential for me to use each kinect coordinate system of human body otherwise it's so difficult to find the cylinder position.
I copy you a part of my code:
#include <ros/ros.h>
#include <arm_navigation_msgs/GetPlanningScene.h>
#include <planning_environment/models/collision_models.h>
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_state_validity_test");
ros::NodeHandle rh;
//vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
//vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);
ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
ros::ServiceClient get_planning_scene_client =
rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
//add the cylinder into the collision space
float add=0;
arm_navigation_msgs::CollisionObject cylinder_object;
arm_navigation_msgs::Shape object;
geometry_msgs::Pose pose;
while(1) {
cylinder_object.id = "pole";
cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
cylinder_object.header.frame_id = "openni_depth_frame"; // it doesn't work
//cylinder_object.header.frame_id = "kinect_link"; //it works
cylinder_object.header.stamp = ros::Time::now();
object.type = arm_navigation_msgs::Shape::CYLINDER;
object.dimensions.resize(2);
object.dimensions[0] = .1;
object.dimensions[1] = 1.0;
pose.position.x = 0.0;
pose.position.y = 0.0;
pose.position.z = 0.0 ;
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
pose.orientation.w = 1;
if (add != 0){
cylinder_object.shapes.pop_back();
cylinder_object.poses.pop_back();
}
cylinder_object.shapes.push_back(object);
cylinder_object.poses.push_back(pose);
planning_scene_req.planning_scene_diff.collision_objects.push_back(cylinder_object);
if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
ROS_WARN("Can't get planning scene");
return -1;
}
ros::spinOnce();
ros::Duration(.1).sleep();
}
ros::shutdown();
}
I check the validity in a other node: It's only a part of my code.
//////////////// Init check trajectory at the beginning
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
ros::ServiceClient get_planning_scene_client =
n.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
ROS_WARN("Can't get planning scene");
}
planning_environment::CollisionModels collision_models("robot_description");
std::map<std::string, double> pos;
//check trajectory In the loop
planning_models::KinematicState* state = collision_models.setPlanningScene(planning_scene_res.planning_scene);
pos[goal->trajectory.joint_names[j]] = goal->trajectory.points[i+pt].positions[j];
state->setKinematicState(pos);
if(collision_models.isKinematicStateInCollision(*state)) {
ROS_WARN("COLLSION !!!");
}
collision_models.revertPlanningScene(state); I used to connect the kinect_link at the openni_depth_frame a link in the launch:
Originally posted by jep31 on ROS Answers with karma: 411 on 2012-07-05
Post score: 0
Original comments
Comment by bit-pirate on 2012-07-10:
Do you just want to stop your arm motion in case a collision between arm and human is detected or do you want to stop the motion and plan a new trajectory around the human?
Comment by bit-pirate on 2012-07-10:
PS: Please add comments to your question (and other people's answers) instead of answers.
Comment by jep31 on 2012-07-11:
At least I would to stop the arm motion when there is a collision between arm and human. Then I could to plan a new trajectory with the same process as the beginning. With a know object the problem is that the collision environment doesn't change during the motion so the check validity is useless.
Comment by jep31 on 2012-07-11:
And with the virtual object, I can't attach objects at coordinate system of human. It's essential for me to use each kinect coordinate system of human body otherwise it's so difficult to find the cylinder position. A example of my code is at the bottom.
Comment by jep31 on 2012-07-15:
I can't comment my answer or other answer without comment and I don't understand why I should edit my post and no answer