robot_model_test_utils.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, Bryce Willey.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of MoveIt nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Bryce Willey */
#pragma once
 
#include <srdfdom/srdf_writer.h>
#if __has_include(<urdf/model.hpp>)
#include <urdf/model.hpp>
#else
#include <urdf/model.h>
#endif
#include <moveit/robot_model/robot_model.hpp>
#include <geometry_msgs/msg/pose.hpp>
 
#include <rclcpp/node.hpp>
 
namespace moveit
{
namespace core
{
 
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& package_name,
                                                  const std::string& urdf_relative_path,
                                                  const std::string& srdf_relative_path);
 
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name);
 
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name);
 
srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name);
 
void loadIKPluginForGroup(const rclcpp::Node::SharedPtr& node, JointModelGroup* jmg, const std::string& base_link,
                          const std::string& tip_link, std::string plugin = "KDL", double timeout = 0.1);
 
class RobotModelBuilder
{
public:
  RobotModelBuilder(const std::string& name, const std::string& base_link_name);
 
  void addChain(const std::string& section, const std::string& type,
                const std::vector<geometry_msgs::msg::Pose>& joint_origins = {},
                urdf::Vector3 joint_axis = urdf::Vector3(1.0, 0.0, 0.0));
 
  void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::msg::Pose origin);
  void addCollisionBox(const std::string& link_name, const std::vector<double>& dims, geometry_msgs::msg::Pose origin);
 
  void addVisualBox(const std::string& link_name, const std::vector<double>& size, geometry_msgs::msg::Pose origin);
 
  void addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy,
                   double ixz, double iyy, double iyz, double izz);
 
  void addVirtualJoint(const std::string& parent_frame, const std::string& child_link, const std::string& type,
                       const std::string& name = "");
 
  void addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name = "");
 
  void addGroup(const std::vector<std::string>& links, const std::vector<std::string>& joints, const std::string& name);
 
  void addEndEffector(const std::string& name, const std::string& parent_link, const std::string& parent_group = "",
                      const std::string& component_group = "");
 
  void addJointProperty(const std::string& joint_name, const std::string& property_name, const std::string& value);
 
  bool isValid();
 
  moveit::core::RobotModelPtr build();
 
private:
  void addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& coll,
                        geometry_msgs::msg::Pose origin);
 
  void addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis, geometry_msgs::msg::Pose origin);
 
  urdf::ModelInterfaceSharedPtr urdf_model_;
  srdf::SRDFWriterPtr srdf_writer_;
 
  bool is_valid_ = true;
};
}  // namespace core
}  // namespace moveit