40 #include <exotica_core/attach_link_initializer.h>
41 #include <exotica_core/box_shape_initializer.h>
42 #include <exotica_core/collision_scene_initializer.h>
43 #include <exotica_core/cylinder_shape_initializer.h>
44 #include <exotica_core/link_initializer.h>
45 #include <exotica_core/mesh_shape_initializer.h>
46 #include <exotica_core/octree_shape_initializer.h>
47 #include <exotica_core/shape_initializer.h>
48 #include <exotica_core/sphere_shape_initializer.h>
49 #include <exotica_core/trajectory_initializer.h>
51 #include <moveit/version.h>
57 Scene::Scene(
const std::string& name) : request_needs_updating_(false)
76 robot_model::RobotModelPtr model;
77 if (
init.URDF ==
"" ||
init.SRDF ==
"")
108 if (
init.LoadScene !=
"")
110 std::vector<std::string> files =
ParseList(
init.LoadScene,
';');
111 for (
const std::string& file : files)
LoadSceneFile(file, Eigen::Isometry3d::Identity(),
false);
117 LinkInitializer link(linkInit);
120 Eigen::Vector4d link_color = Eigen::Vector4d::Zero();
121 if (link.Shape.size() == 1)
123 ShapeInitializer shape(link.Shape[0]);
124 link_color = shape.Color;
126 if (shape.Type ==
"Box")
128 BoxShapeInitializer box(link.Shape[0]);
129 std::cout <<
"BOX: " << box.Dimensions.transpose() << std::endl;
130 link_shape.reset(
new shapes::Box(box.Dimensions.x(), box.Dimensions.y(), box.Dimensions.z()));
132 else if (shape.Type ==
"Cylinder")
134 CylinderShapeInitializer cylinder(link.Shape[0]);
137 else if (shape.Type ==
"Mesh")
139 MeshShapeInitializer mesh(link.Shape[0]);
143 else if (shape.Type ==
"Octree")
145 OctreeShapeInitializer octree(link.Shape[0]);
148 else if (shape.Type ==
"Sphere")
150 SphereShapeInitializer sphere(link.Shape[0]);
155 ThrowPretty(
"Unrecognized ShapeType: " << shape.Type);
158 else if (link.Shape.size() > 1)
160 ThrowPretty(
"Only one Shape per Link allowed, given: " << link.Shape.size());
163 AddObject(link.Name,
GetFrame(link.Transform), link.Parent, link_shape, KDL::RigidBodyInertia(link.Mass,
GetFrame(link.CenterOfMass).p), link_color,
false);
167 if (
init.RobotLinksToExcludeFromCollisionScene.size() > 0)
169 for (
const auto& link :
init.RobotLinksToExcludeFromCollisionScene)
177 if (
init.WorldLinksToExcludeFromCollisionScene.size() > 0)
179 for (
const auto& link :
init.WorldLinksToExcludeFromCollisionScene)
188 if (!
init.DoNotInstantiateCollisionScene)
190 if (
init.CollisionScene.size() == 0)
195 else if (
init.CollisionScene.size() == 1)
202 ThrowPretty(
"Only one CollisionScene per scene allowed - " <<
init.CollisionScene.size() <<
" provided");
218 AttachLinkInitializer link(linkInit);
231 std::vector<std::string> acm_names;
232 ps_->getAllowedCollisionMatrix().getAllEntryNames(acm_names);
233 for (
auto& name1 : acm_names)
235 for (
auto& name2 : acm_names)
238 ps_->getAllowedCollisionMatrix().getAllowedCollision(name1, name2,
type);
239 if (
type == collision_detection::AllowedCollision::Type::ALWAYS)
250 TrajectoryInitializer trajInit(it);
251 if (trajInit.File !=
"")
262 if (
init.DynamicsSolver.size() > 0)
265 if (
init.DynamicsSolver.size() > 1)
ThrowPretty(
"Only one DynamicsSolver per scene allowed - " <<
init.DynamicsSolver.size() <<
" provided");
307 it.second.first.lock()->generated_offset = it.second.second->GetPosition(
t);
327 for (
const auto& joint : modelState)
331 ps_->getCurrentStateNonConst().setVariablePosition(joint.first, joint.second);
333 catch (
const std::out_of_range& e)
335 HIGHLIGHT(
"Could not find Kinematica joint name in MoveIt: " + joint.first);
347 Eigen::Quaterniond quat(Eigen::Map<const Eigen::Matrix3d>(rot.data).transpose());
371 visualization_msgs::Marker
Scene::ProxyToMarker(
const std::vector<CollisionProxy>& proxies,
const std::string& frame)
373 visualization_msgs::Marker ret;
374 ret.header.frame_id =
"exotica/" + frame;
375 ret.action = visualization_msgs::Marker::ADD;
376 ret.frame_locked =
false;
380 ret.type = visualization_msgs::Marker::LINE_LIST;
381 ret.points.resize(proxies.size() * 6);
382 ret.colors.resize(proxies.size() * 6);
384 ret.pose.orientation.w = 1.0;
385 double normalLength = 0.01;
386 std_msgs::ColorRGBA normal =
GetColor(0.8, 0.8, 0.8);
387 std_msgs::ColorRGBA far =
GetColor(0.5, 0.5, 0.5);
388 std_msgs::ColorRGBA colliding =
GetColor(1, 0, 0);
389 for (
int i = 0; i < proxies.size(); ++i)
391 KDL::Vector c1 = KDL::Vector(proxies[i].contact1(0), proxies[i].contact1(1), proxies[i].contact1(2));
392 KDL::Vector c2 = KDL::Vector(proxies[i].contact2(0), proxies[i].contact2(1), proxies[i].contact2(2));
393 KDL::Vector n1 = KDL::Vector(proxies[i].normal1(0), proxies[i].normal1(1), proxies[i].normal1(2));
394 KDL::Vector n2 = KDL::Vector(proxies[i].normal2(0), proxies[i].normal2(1), proxies[i].normal2(2));
401 ret.colors[i * 6] = ret.colors[i * 6 + 1] = proxies[i].distance > 0 ? far : colliding;
402 ret.colors[i * 6 + 2] = ret.colors[i * 6 + 3] = ret.colors[i * 6 + 4] = ret.colors[i * 6 + 5] = normal;
409 ps_->usePlanningSceneMsg(scene);
416 ps_->processPlanningSceneWorldMsg(*world);
454 moveit_msgs::PlanningScene msg;
455 ps_->getPlanningSceneMsg(msg);
460 msg.link_padding.clear();
461 msg.link_scale.clear();
462 for (
auto robot_link : msg.robot_state.joint_state.name)
464 moveit_msgs::LinkPadding padding;
465 padding.link_name = robot_link;
467 msg.link_padding.push_back(padding);
469 moveit_msgs::LinkScale scale;
470 scale.link_name = robot_link;
472 msg.link_scale.push_back(scale);
480 for (
auto it : msg.world.collision_objects)
483 for (
auto primitive : it.primitives)
489 primitive = boost::get<shape_msgs::SolidPrimitive>(tmp_msg);
493 for (
auto mesh : it.meshes)
499 mesh = boost::get<shape_msgs::Mesh>(tmp_msg);
586 Eigen::Isometry3d tmp_offset;
588 LoadScene(scene, tmp_offset, update_collision_scene);
593 std::stringstream ss(scene);
599 Eigen::Isometry3d tmp_offset;
601 LoadSceneFile(file_name, tmp_offset, update_collision_scene);
613 #if ROS_VERSION_MINIMUM(1, 14, 0) // if ROS version >= ROS_MELODIC
616 ps_->loadGeometryFromStream(in, Eigen::Affine3d(
offset));
625 std::stringstream ss;
626 ps_->saveGeometryToStream(ss);
633 ps_->removeAllCollisionObjects();
642 Eigen::Isometry3d pose;
644 std::string shape_resource_path = it->shape_resource_path;
645 Eigen::Vector3d scale = it->scale;
646 it =
kinematica_.
AddElement(it->segment.getName(), pose, it->parent_name, it->shape, it->segment.getInertia(), it->color, it->visual, it->is_controlled);
647 it->shape_resource_path = shape_resource_path;
653 for (
auto& traj : trajectory_generators_copy)
681 std::map<std::string, int> visual_map;
682 for (
const auto&
object : *
ps_->getWorld())
684 if (
object.second->shapes_.size())
687 Eigen::Isometry3d obj_transform;
688 obj_transform.translation() =
object.second->shape_poses_[0].translation();
689 obj_transform.linear() =
object.second->shape_poses_[0].rotation();
691 std::vector<VisualElement> visuals;
692 for (
int i = 0; i <
object.second->shape_poses_.size(); ++i)
694 Eigen::Isometry3d shape_transform;
695 shape_transform.translation() =
object.second->shape_poses_[i].translation();
696 shape_transform.linear() =
object.second->shape_poses_[i].rotation();
697 Eigen::Isometry3d trans = obj_transform.inverse() * shape_transform;
699 std::string
name =
object.first;
701 const auto& it = visual_map.find(
name);
702 if (it != visual_map.end())
705 name =
name +
"_" + std::to_string(it->second);
709 visual_map[
name] = 0;
714 if (
ps_->hasObjectColor(
object.first))
716 auto color_msg =
ps_->getObjectColor(
object.first);
717 visual.
color = Eigen::Vector4d(color_msg.r, color_msg.g, color_msg.b, color_msg.a);
724 if (visual.
shape) visuals.push_back(visual);
726 element->visual = visuals;
730 HIGHLIGHT(
"Object with no shapes ('" <<
object.first <<
"')");
735 ps_->getCurrentStateNonConst().update(
true);
736 #if MOVEIT_VERSION_MAJOR >= 1 && MOVEIT_VERSION_MINOR >= 1
737 const std::vector<const robot_model::LinkModel*>& links =
ps_->getCollisionEnv()->getRobotModel()->getLinkModelsWithCollisionGeometry();
739 const std::vector<const robot_model::LinkModel*>& links =
ps_->getCollisionRobot()->getRobotModel()->getLinkModelsWithCollisionGeometry();
742 int last_controlled_joint_id = -1;
743 std::string last_controlled_joint_name =
"";
747 for (
int i = 0; i < links.size(); ++i)
755 Eigen::Isometry3d obj_transform;
756 obj_transform.translation() =
ps_->getCurrentState().getGlobalLinkTransform(links[i]).translation();
757 obj_transform.linear() =
ps_->getCurrentState().getGlobalLinkTransform(links[i]).rotation();
762 if (last_controlled_joint_id != joint_id)
765 last_controlled_joint_id = joint_id;
769 for (
int j = 0; j < links[i]->getShapes().size(); ++j)
778 Eigen::Affine3d collisionBodyTransform_affine =
ps_->getCurrentState().getCollisionBodyTransform(links[i], j);
779 Eigen::Isometry3d collisionBodyTransform;
780 collisionBodyTransform.translation() = collisionBodyTransform_affine.translation();
781 collisionBodyTransform.linear() = collisionBodyTransform_affine.rotation();
782 Eigen::Isometry3d trans = obj_transform.inverse(Eigen::Isometry) * collisionBodyTransform;
784 std::string collision_element_name = links[i]->getName() +
"_collision_" + std::to_string(j);
785 std::shared_ptr<KinematicElement> element =
kinematica_.
AddElement(collision_element_name, trans, links[i]->
getName(), links[i]->getShapes()[j]);
791 if (last_controlled_joint_name !=
"")
803 void Scene::AddObject(
const std::string& name,
const KDL::Frame& transform,
const std::string& parent,
shapes::ShapeConstPtr shape,
const KDL::RigidBodyInertia& inertia,
const Eigen::Vector4d& color,
bool update_collision_scene)
808 Eigen::Isometry3d pose;
814 void Scene::AddObject(
const std::string& name,
const KDL::Frame& transform,
const std::string& parent,
const std::string& shape_resource_path,
const Eigen::Vector3d& scale,
const KDL::RigidBodyInertia& inertia,
const Eigen::Vector4d& color,
bool update_collision_scene)
819 Eigen::Isometry3d pose;
831 throw std::runtime_error(
"link '" +
name +
"' already exists in kinematic tree");
833 Eigen::Isometry3d pose;
835 ps_->getWorldNonConst()->addToObject(
name, shape, pose);
846 if ((*it)->segment.getName() ==
name)
904 const auto& it = tree.find(link);
905 if (it == tree.end())
ThrowPretty(
"Can't find link '" << link <<
"'!");
906 if (traj->GetDuration() == 0.0)
ThrowPretty(
"The trajectory is empty!");
907 trajectory_generators_[link] = std::pair<std::weak_ptr<KinematicElement>, std::shared_ptr<Trajectory>>(it->second, traj);
908 it->second.lock()->is_trajectory_generated =
true;
915 return it->second.second;
922 it->second.first.lock()->is_trajectory_generated =
false;