nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove example code

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-01-10 16:15:42 +02:00
parent 0e2efc3980
commit 1d2cc754a7
16 changed files with 7 additions and 2381 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
CMakeLists.txt
View File

@@ -27,31 +27,9 @@ find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
# Install C++ examples
set(EXAMPLES_CPP_DIR examples/cpp)
# Example 0 - Follow target
set(EXECUTABLE_0 ex_follow_target)
add_executable(${EXECUTABLE_0} ${EXAMPLES_CPP_DIR}/${EXECUTABLE_0}.cpp)
ament_target_dependencies(${EXECUTABLE_0}
rclcpp
geometry_msgs
moveit_ros_planning_interface
)
install(TARGETS
${EXECUTABLE_0}
DESTINATION lib/${PROJECT_NAME}
)
# Install Python examples
set(EXAMPLES_PY_DIR examples/py)
install(PROGRAMS
${EXAMPLES_PY_DIR}/ex_follow_target.py
${EXAMPLES_PY_DIR}/ex_throw_object.py
DESTINATION lib/${PROJECT_NAME}
)
# Install directories
install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME})
#install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME})
# Setup the project
ament_package()

71
examples/cpp/ex_follow_target.cpp
View File

@@ -1,71 +0,0 @@
/// Example that uses MoveIt 2 to follow a target inside Ignition Gazebo
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <rclcpp/qos.hpp>
#include <rclcpp/rclcpp.hpp>
const std::string MOVE_GROUP = "arm";
class MoveItFollowTarget : public rclcpp::Node
{
public:
/// Constructor
MoveItFollowTarget();
/// Move group interface for the robot
moveit::planning_interface::MoveGroupInterface move_group_;
/// Subscriber for target pose
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
/// Target pose that is used to detect changes
geometry_msgs::msg::Pose previous_target_pose_;
private:
/// Callback that plans and executes trajectory each time the target pose is changed
void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
};
MoveItFollowTarget::MoveItFollowTarget() : Node("ex_follow_target"),
move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
{
// Use upper joint velocity and acceleration limits
this->move_group_.setMaxAccelerationScalingFactor(1.0);
this->move_group_.setMaxVelocityScalingFactor(1.0);
// Subscribe to target pose
target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "Initialization successful.");
}
void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
{
// Return if target pose is unchanged
if (msg->pose == previous_target_pose_)
{
return;
}
RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
// Plan and execute motion
this->move_group_.setPoseTarget(msg->pose);
this->move_group_.move();
// Update for next callback
previous_target_pose_ = msg->pose;
}
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto target_follower = std::make_shared<MoveItFollowTarget>();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(target_follower);
executor.spin();
rclcpp::shutdown();
return EXIT_SUCCESS;
}

83
examples/py/ex_follow_target.py
View File

@@ -1,83 +0,0 @@
#!/usr/bin/env python3
"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo"""
import rclpy
from geometry_msgs.msg import Pose, PoseStamped
from pymoveit2 import MoveIt2
from pymoveit2.robots import panda
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node
from rclpy.qos import QoSProfile
class MoveItFollowTarget(Node):
def __init__(self):
super().__init__("ex_follow_target_py")
# Create callback group that allows execution of callbacks in parallel without restrictions
self._callback_group = ReentrantCallbackGroup()
# Create MoveIt 2 interface
self._moveit2 = MoveIt2(
node=self,
joint_names=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.MOVE_GROUP_ARM,
execute_via_moveit=True,
callback_group=self._callback_group,
)
# Use upper joint velocity and acceleration limits
self._moveit2.max_velocity = 1.0
self._moveit2.max_acceleration = 1.0
# Create a subscriber for target pose
self.__previous_target_pose = Pose()
self.create_subscription(
msg_type=PoseStamped,
topic="/target_pose",
callback=self.target_pose_callback,
qos_profile=QoSProfile(depth=1),
callback_group=self._callback_group,
)
self.get_logger().info("Initialization successful.")
def target_pose_callback(self, msg: PoseStamped):
"""
Plan and execute trajectory each time the target pose is changed
"""
# Return if target pose is unchanged
if msg.pose == self.__previous_target_pose:
return
self.get_logger().info("Target pose has changed. Planning and executing...")
# Plan and execute motion
self._moveit2.move_to_pose(
position=msg.pose.position,
quat_xyzw=msg.pose.orientation,
)
# Update for next callback
self.__previous_target_pose = msg.pose
def main(args=None):
rclpy.init(args=args)
target_follower = MoveItFollowTarget()
executor = rclpy.executors.MultiThreadedExecutor(2)
executor.add_node(target_follower)
executor.spin()
rclpy.shutdown()
exit(0)
if __name__ == "__main__":
main()

151
examples/py/ex_throw_object.py
View File

@@ -1,151 +0,0 @@
#!/usr/bin/env python3
from copy import deepcopy
from threading import Thread
import rclpy
from geometry_msgs.msg import Point, Quaternion
from pymoveit2 import MoveIt2, MoveIt2Gripper
from pymoveit2.robots import panda
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node
class MoveItThrowObject(Node):
def __init__(self):
super().__init__("ex_throw_object")
# Create callback group that allows execution of callbacks in parallel without restrictions
self._callback_group = ReentrantCallbackGroup()
# Create MoveIt 2 interface
self._moveit2 = MoveIt2(
node=self,
joint_names=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.MOVE_GROUP_ARM,
callback_group=self._callback_group,
)
# Use upper joint velocity and acceleration limits
self._moveit2.max_velocity = 1.0
self._moveit2.max_acceleration = 1.0
# Create MoveIt 2 interface for gripper
self._moveit2_gripper = MoveIt2Gripper(
node=self,
gripper_joint_names=panda.gripper_joint_names(),
open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS,
closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
gripper_group_name=panda.MOVE_GROUP_GRIPPER,
callback_group=self._callback_group,
)
self.get_logger().info("Initialization successful.")
def throw(self):
"""
Plan and execute hard-coded trajectory with intention to throw an object
"""
self.get_logger().info("Throwing... Wish me luck!")
throwing_object_pos = Point(x=0.5, y=0.0, z=0.015)
default_quat = Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)
# Open gripper
self._moveit2_gripper.open()
self._moveit2_gripper.wait_until_executed()
# Move above object
position_above_object = deepcopy(throwing_object_pos)
position_above_object.z += 0.15
self._moveit2.move_to_pose(
position=position_above_object,
quat_xyzw=default_quat,
)
self._moveit2.wait_until_executed()
# Move to grasp position
self._moveit2.move_to_pose(
position=throwing_object_pos,
quat_xyzw=default_quat,
)
self._moveit2.wait_until_executed()
# Close gripper
self._moveit2_gripper.close()
self._moveit2_gripper.wait_until_executed()
# Decrease speed
self._moveit2.max_velocity = 0.25
self._moveit2.max_acceleration = 0.1
# Move above object (again)
self._moveit2.move_to_pose(
position=position_above_object,
quat_xyzw=default_quat,
)
self._moveit2.wait_until_executed()
# Move to pre-throw configuration
joint_configuration_pre_throw = [0.0, -1.75, 0.0, -0.1, 0.0, 3.6, 0.8]
self._moveit2.move_to_configuration(joint_configuration_pre_throw)
self._moveit2.wait_until_executed()
# Increase speed
self._moveit2.max_velocity = 1.0
self._moveit2.max_acceleration = 1.0
# Throw itself
joint_configuration_throw = [0.0, 1.0, 0.0, -1.1, 0.0, 1.9, 0.8]
self._moveit2.move_to_configuration(joint_configuration_throw)
# Release object while executing motion
sleep_duration_s = 1.2
if rclpy.ok():
self.create_rate(1 / sleep_duration_s).sleep()
self._moveit2_gripper.open()
self._moveit2_gripper.wait_until_executed()
self._moveit2.wait_until_executed()
# Return to default configuration
joint_configuration_default = [
0.0,
-0.7853981633974483,
0.0,
-2.356194490192345,
0.0,
1.5707963267948966,
0.7853981633974483,
]
self._moveit2.move_to_configuration(joint_configuration_default)
self._moveit2.wait_until_executed()
def main(args=None):
rclpy.init(args=args)
object_thrower = MoveItThrowObject()
# Spin the node in background thread(s)
executor = rclpy.executors.MultiThreadedExecutor(3)
executor.add_node(object_thrower)
executor_thread = Thread(target=executor.spin, daemon=True, args=())
executor_thread.start()
# Wait for everything to setup
sleep_duration_s = 2.0
if rclpy.ok():
object_thrower.create_rate(1 / sleep_duration_s).sleep()
object_thrower.throw()
rclpy.shutdown()
exit(0)
if __name__ == "__main__":
main()

150
launch/default.launch.py
View File

@@ -1,150 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch default world with the default robot (configurable)"""
from os import path
from typing import List
from ament_index_python.packages import get_package_share_directory
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
world_type = LaunchConfiguration("world_type")
robot_type = LaunchConfiguration("robot_type")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# Determine what world/robot combination to launch
declared_arguments.append(
DeclareLaunchArgument(
"__world_launch_basename",
default_value=["world_", world_type, ".launch.py"],
),
)
declared_arguments.append(
DeclareLaunchArgument(
"__robot_launch_basename",
default_value=["robot_", robot_type, ".launch.py"],
),
)
# List of included launch descriptions
launch_descriptions = [
# Launch Ignition Gazebo with the required ROS<->IGN bridges
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ign_moveit2_examples"),
"launch",
"worlds",
LaunchConfiguration("__world_launch_basename"),
]
)
),
launch_arguments=[
("use_sim_time", use_sim_time),
("ign_verbosity", ign_verbosity),
("log_level", log_level),
],
),
# Spawn robot
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ign_moveit2_examples"),
"launch",
"robots",
LaunchConfiguration("__robot_launch_basename"),
]
)
),
launch_arguments=[
("use_sim_time", use_sim_time),
("ign_verbosity", ign_verbosity),
("log_level", log_level),
],
),
# Launch move_group of MoveIt 2
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare([robot_type, "_moveit_config"]),
"launch",
"move_group.launch.py",
]
)
),
launch_arguments=[
("ros2_control_plugin", "ign"),
("ros2_control_command_interface", "effort"),
# TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled
("collision_arm", "false"),
("rviz_config", rviz_config),
("use_sim_time", use_sim_time),
("log_level", log_level),
],
),
]
return LaunchDescription(declared_arguments + launch_descriptions)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# World selection
DeclareLaunchArgument(
"world_type",
default_value="default",
description="Name of the world configuration to load.",
),
# Robot selection
DeclareLaunchArgument(
"robot_type",
default_value="panda",
description="Name of the robot type to use.",
),
# Miscellaneous
DeclareLaunchArgument(
"rviz_config",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"rviz",
"ign_moveit2_examples.rviz",
),
description="Path to configuration for RViz2.",
),
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

187
launch/ex_cpp_follow_target.launch.py
View File

@@ -1,187 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch C++ example for following a target"""
from os import path
from typing import List
import yaml
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
description_package = LaunchConfiguration("description_package")
description_filepath = LaunchConfiguration("description_filepath")
moveit_config_package = "panda_moveit_config"
robot_type = LaunchConfiguration("robot_type")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# URDF
_robot_description_xml = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), description_filepath]
),
" ",
"name:=",
robot_type,
]
)
robot_description = {"robot_description": _robot_description_xml}
# SRDF
_robot_description_semantic_xml = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare(moveit_config_package),
"srdf",
"panda.srdf.xacro",
]
),
" ",
"name:=",
robot_type,
]
)
robot_description_semantic = {
"robot_description_semantic": _robot_description_semantic_xml
}
# List of included launch descriptions
launch_descriptions = [
# Launch world with robot (configured for this example)
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ign_moveit2_examples"),
"launch",
"default.launch.py",
]
)
),
launch_arguments=[
("world_type", "follow_target"),
("robot_type", robot_type),
("rviz_config", rviz_config),
("use_sim_time", use_sim_time),
("ign_verbosity", ign_verbosity),
("log_level", log_level),
],
),
]
# List of nodes to be launched
nodes = [
# Run the example node (C++)
Node(
package="ign_moveit2_examples",
executable="ex_follow_target",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
robot_description_semantic,
{"use_sim_time": use_sim_time},
],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def load_yaml(package_name: str, file_path: str):
"""
Load yaml configuration based on package name and file path relative to its share.
"""
package_path = get_package_share_directory(package_name)
absolute_file_path = path.join(package_path, file_path)
return parse_yaml(absolute_file_path)
def parse_yaml(absolute_file_path: str):
"""
Parse yaml from file, given its absolute file path.
"""
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError:
return None
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# Locations of robot resources
DeclareLaunchArgument(
"description_package",
default_value="panda_description",
description="Custom package with robot description.",
),
DeclareLaunchArgument(
"description_filepath",
default_value=path.join("urdf", "panda.urdf.xacro"),
description="Path to xacro or URDF description of the robot, relative to share of `description_package`.",
),
# Robot selection
DeclareLaunchArgument(
"robot_type",
default_value="panda",
description="Name of the robot to use.",
),
# Miscellaneous
DeclareLaunchArgument(
"rviz_config",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"rviz",
"ign_moveit2_examples.rviz",
),
description="Path to configuration for RViz2.",
),
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

105
launch/ex_py_follow_target.launch.py
View File

@@ -1,105 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch Python example for following a target"""
from os import path
from typing import List
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
robot_type = LaunchConfiguration("robot_type")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# List of included launch descriptions
launch_descriptions = [
# Launch world with robot (configured for this example)
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ign_moveit2_examples"),
"launch",
"default.launch.py",
]
)
),
launch_arguments=[
("world_type", "follow_target"),
("robot_type", robot_type),
("rviz_config", rviz_config),
("use_sim_time", use_sim_time),
("ign_verbosity", ign_verbosity),
("log_level", log_level),
],
),
]
# List of nodes to be launched
nodes = [
# Run the example node (Python)
Node(
package="ign_moveit2_examples",
executable="ex_follow_target.py",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# Robot selection
DeclareLaunchArgument(
"robot_type",
default_value="panda",
description="Name of the robot to use.",
),
# Miscellaneous
DeclareLaunchArgument(
"rviz_config",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"rviz",
"ign_moveit2_examples.rviz",
),
description="Path to configuration for RViz2.",
),
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

105
launch/ex_py_throw_object.launch.py
View File

@@ -1,105 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch Python example for throwing an object"""
from os import path
from typing import List
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
robot_type = LaunchConfiguration("robot_type")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# List of included launch descriptions
launch_descriptions = [
# Launch world with robot (configured for this example)
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ign_moveit2_examples"),
"launch",
"default.launch.py",
]
)
),
launch_arguments=[
("world_type", "throw_object"),
("robot_type", robot_type),
("rviz_config", rviz_config),
("use_sim_time", use_sim_time),
("ign_verbosity", ign_verbosity),
("log_level", log_level),
],
),
]
# List of nodes to be launched
nodes = [
# Run the example node (Python)
Node(
package="ign_moveit2_examples",
executable="ex_throw_object.py",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# Robot selection
DeclareLaunchArgument(
"robot_type",
default_value="panda",
description="Name of the robot to use.",
),
# Miscellaneous
DeclareLaunchArgument(
"rviz_config",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"rviz",
"ign_moveit2_examples.rviz",
),
description="Path to configuration for RViz2.",
),
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

61
launch/robots/robot_panda.launch.py
View File

@@ -1,61 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch script for spawning Franka Emika Panda into Ignition Gazebo world"""
from typing import List
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
model = LaunchConfiguration("model")
use_sim_time = LaunchConfiguration("use_sim_time")
log_level = LaunchConfiguration("log_level")
# List of nodes to be launched
nodes = [
# ros_ign_gazebo_create
Node(
package="ros_ign_gazebo",
executable="create",
output="log",
arguments=["-file", model, "--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
),
]
return LaunchDescription(declared_arguments + nodes)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# Model for Ignition Gazebo
DeclareLaunchArgument(
"model",
default_value="panda",
description="Name or filepath of model to load.",
),
# Miscellaneous
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

91
launch/worlds/world_default.launch.py
View File

@@ -1,91 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch default.sdf and the required ROS<->IGN bridges"""
from typing import List
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
world = LaunchConfiguration("world")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# List of included launch descriptions
launch_descriptions = [
# Launch Ignition Gazebo
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ros_ign_gazebo"),
"launch",
"ign_gazebo.launch.py",
]
)
),
launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
),
]
# List of nodes to be launched
nodes = [
# ros_ign_bridge (clock -> ROS 2)
Node(
package="ros_ign_bridge",
executable="parameter_bridge",
output="log",
arguments=[
"/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
"--ros-args",
"--log-level",
log_level,
],
parameters=[{"use_sim_time": use_sim_time}],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# World for Ignition Gazebo
DeclareLaunchArgument(
"world",
default_value="default.sdf",
description="Name or filepath of world to load.",
),
# Miscellaneous
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

113
launch/worlds/world_follow_target.launch.py
View File

@@ -1,113 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges"""
from os import path
from typing import List
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
world = LaunchConfiguration("world")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# List of included launch descriptions
launch_descriptions = [
# Launch Ignition Gazebo
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ros_ign_gazebo"),
"launch",
"ign_gazebo.launch.py",
]
)
),
launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
),
]
# List of nodes to be launched
nodes = [
# ros_ign_bridge (clock -> ROS 2)
Node(
package="ros_ign_bridge",
executable="parameter_bridge",
output="log",
arguments=[
"/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
"--ros-args",
"--log-level",
log_level,
],
parameters=[{"use_sim_time": use_sim_time}],
),
# ros_ign_bridge (target pose -> ROS 2)
Node(
package="ros_ign_bridge",
executable="parameter_bridge",
output="log",
arguments=[
"/model/target/pose"
+ "@"
+ "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose",
"--ros-args",
"--log-level",
log_level,
],
parameters=[{"use_sim_time": use_sim_time}],
remappings=[("/model/target/pose", "/target_pose")],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# World for Ignition Gazebo
DeclareLaunchArgument(
"world",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"worlds",
"follow_target.sdf",
),
description="Name or filepath of world to load.",
),
# Miscellaneous
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

97
launch/worlds/world_throw_object.launch.py
View File

@@ -1,97 +0,0 @@
#!/usr/bin/env -S ros2 launch
"""Launch worlds/throw_object.sdf and the required ROS<->IGN bridges"""
from os import path
from typing import List
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
world = LaunchConfiguration("world")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# List of included launch descriptions
launch_descriptions = [
# Launch Ignition Gazebo
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[
FindPackageShare("ros_ign_gazebo"),
"launch",
"ign_gazebo.launch.py",
]
)
),
launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
),
]
# List of nodes to be launched
nodes = [
# ros_ign_bridge (clock -> ROS 2)
Node(
package="ros_ign_bridge",
executable="parameter_bridge",
output="log",
arguments=[
"/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
"--ros-args",
"--log-level",
log_level,
],
parameters=[{"use_sim_time": use_sim_time}],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
"""
Generate list of all launch arguments that are declared for this launch script.
"""
return [
# World for Ignition Gazebo
DeclareLaunchArgument(
"world",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"worlds",
"throw_object.sdf",
),
description="Name or filepath of world to load.",
),
# Miscellaneous
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="If true, use simulated clock.",
),
DeclareLaunchArgument(
"ign_verbosity",
default_value="2",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
]

12
package.xml
View File

@@ -1,11 +1,11 @@
<?xml version="1.0"?>
<package format="3">
<name>ign_moveit2_examples</name>
<version>1.0.0</version>
<description>C++ and Python examples of using MoveIt 2 for planning motions that are executed inside Ignition Gazebo simulation environment</description>
<maintainer email="orsula.andrej@gmail.com">Andrej Orsula</maintainer>
<author>Andrej Orsula</author>
<license>BSD</license>
<name>drawing-robot-ros2</name>
<version>0.0.0</version>
<description>drawing-robot-ros2</description>
<maintainer email=""></maintainer>
<author></author>
<license></license>
<buildtool_depend>ament_cmake</buildtool_depend>

337
rviz/ign_moveit2_examples.rviz
View File

@@ -1,337 +0,0 @@
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /MotionPlanning1
Splitter Ratio: 0.6278260946273804
Tree Height: 412
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Acceleration_Scaling_Factor: 1
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
MoveIt_Allow_Approximate_IK: false
MoveIt_Allow_External_Program: false
MoveIt_Allow_Replanning: false
MoveIt_Allow_Sensor_Positioning: false
MoveIt_Planning_Attempts: 10
MoveIt_Planning_Time: 5
MoveIt_Use_Cartesian_Path: false
MoveIt_Use_Constraint_Aware_IK: false
MoveIt_Warehouse_Host: 127.0.0.1
MoveIt_Warehouse_Port: 33829
MoveIt_Workspace:
Center:
X: 0
Y: 0
Z: 0
Size:
X: 2
Y: 2
Z: 2
Name: MotionPlanning
Planned Path:
Color Enabled: false
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_hand_tcp:
Alpha: 1
Show Axes: false
Show Trail: false
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
Loop Animation: true
Robot Alpha: 0.25
Robot Color: 150; 50; 150
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.1s
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
TextHeight: 0.07999999821186066
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 0.5
Goal State Color: 250; 128; 0
Interactive Marker Size: 0.10000000149011612
Joint Violation Color: 255; 0; 255
Planning Group: arm
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 0.5
Start State Color: 0; 255; 0
Planning Scene Topic: monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_hand_tcp:
Alpha: 1
Show Axes: false
Show Trail: false
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
Velocity_Scaling_Factor: 1
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: panda_link0
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.5
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.3333333432674408
Y: 0
Z: 0.5
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.39269909262657166
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.7853981852531433
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 720
Hide Left Dock: false
Hide Right Dock: true
MotionPlanning:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000247000003b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000001eb000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000232000001c4000001b9010000030000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000003b0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000538000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1280
X: 0
Y: 0

108
worlds/follow_target.sdf
View File

@@ -1,108 +0,0 @@
<?xml version="1.0"?>
<sdf version="1.9">
<world name="ign_moveit2_follow_target_world">
<!-- Physics -->
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
<engine>
<filename>ignition-physics-dartsim-plugin</filename>
</engine>
<dart>
<collision_detector>bullet</collision_detector>
</dart>
</plugin>
<physics name="physics_config" type="dart">
<max_step_size>0.005</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<!-- Scene -->
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<scene>
<ambient>0.8 0.8 0.8</ambient>
<grid>false</grid>
</scene>
<!-- User Commands (transform control) -->
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
</plugin>
<!-- -->
<!-- Illumination -->
<!-- -->
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.3 0.3 -0.9</direction>
</light>
<!-- -->
<!-- Models -->
<!-- -->
<!-- Ground -->
<model name="ground_plane">
<pose>0 0 0 0 0 0</pose>
<static>true</static>
<link name="ground_plane_link">
<collision name="ground_plane_collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="ground_plane_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>4 4</size>
</plane>
</geometry>
<material>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<!-- Static target -->
<model name="target">
<static>true</static>
<pose>0.5 -0.25 0.5 3.1415927 0 0</pose>
<link name="target_link">
<visual name="target_visual">
<cast_shadows>false</cast_shadows>
<geometry>
<box>
<size>0.04 0.04 0.04</size>
</box>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<specular>0.4 0.4 0.4 1</specular>
</material>
</visual>
</link>
<!-- Pose publisher plugin, used to determine the target pose to reach -->
<plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
<publish_nested_model_pose>true</publish_nested_model_pose>
<publish_link_pose>false</publish_link_pose>
<publish_collision_pose>false</publish_collision_pose>
<publish_visual_pose>false</publish_visual_pose>
</plugin>
</model>
</world>
</sdf>

693
worlds/throw_object.sdf
View File

@@ -1,693 +0,0 @@
<?xml version="1.0"?>
<sdf version="1.9">
<world name="ign_moveit2_follow_target_world">
<!-- Physics -->
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
<engine>
<filename>ignition-physics-dartsim-plugin</filename>
</engine>
<dart>
<collision_detector>bullet</collision_detector>
</dart>
</plugin>
<physics name="physics_config" type="dart">
<max_step_size>0.002</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<!-- Scene -->
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<scene>
<ambient>0.8 0.8 0.8</ambient>
<grid>false</grid>
</scene>
<!-- User Commands (transform control) -->
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
</plugin>
<!-- -->
<!-- Illumination -->
<!-- -->
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.3 0.3 -0.9</direction>
</light>
<!-- -->
<!-- Models -->
<!-- -->
<!-- Ground -->
<model name="ground_plane">
<pose>0.5 0 0 0 0 0</pose>
<static>true</static>
<link name="ground_plane_link">
<collision name="ground_plane_collision">
<pose>0 0 -0.05 0 0 0</pose>
<geometry>
<box>
<size>5 2 0.1</size>
</box>
</geometry>
</collision>
<visual name="ground_plane_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>5 2</size>
</plane>
</geometry>
<material>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="underground_plane">
<pose>0.5 0 -10.0 0 0 0</pose>
<static>true</static>
<link name="underground_plane_link">
<collision name="underground_plane_collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
</link>
</model>
<!-- Blockade -->
<model name="blockade">
<pose>2.95 0 0.5 0 0 0</pose>
<static>true</static>
<link name="blockade_link">
<collision name="blockade_collision">
<geometry>
<box>
<size>0.1 2.0 1.0</size>
</box>
</geometry>
</collision>
<visual name="blockade_visual">
<geometry>
<box>
<size>0.1 2.0 1.0</size>
</box>
</geometry>
<material>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<!-- Object to throw -->
<model name="throwing_object">
<pose>0.5 0 0.025 1.5707963 0 0</pose>
<link name="throwing_object_link">
<inertial>
<inertia>
<ixx>0.00025</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00025</iyy>
<iyz>0.0</iyz>
<izz>0.00025</izz>
</inertia>
<mass>0.6</mass>
</inertial>
<collision name="throwing_object_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>2.0</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="throwing_object_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<specular>0.4 0.4 0.4 1</specular>
</material>
</visual>
</link>
</model>
<!-- Target pyramid -->
<!-- Re-enable boxes if you want a larger pyramid -->
<!-- <model name="pyramid_box0">
<pose>1.5 -0.15 0.025 0 0 0</pose>
<link name="pyramid_box0_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box0_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box0_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<model name="pyramid_box1">
<pose>1.5 -0.075 0.025 0 0 0</pose>
<link name="pyramid_box1_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box1_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box1_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<model name="pyramid_box2">
<pose>1.5 0 0.025 0 0 0</pose>
<link name="pyramid_box2_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box2_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box2_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<model name="pyramid_box3">
<pose>1.5 0.075 0.025 0 0 0</pose>
<link name="pyramid_box3_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box3_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box3_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<!-- <model name="pyramid_box4">
<pose>1.5 0.15 0.025 0 0 0</pose>
<link name="pyramid_box4_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box4_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box4_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<!-- <model name="pyramid_box5">
<pose>1.5 -0.1125 0.075 0 0 0</pose>
<link name="pyramid_box5_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box5_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box5_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<model name="pyramid_box6">
<pose>1.5 -0.0375 0.075 0 0 0</pose>
<link name="pyramid_box6_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box6_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box6_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<model name="pyramid_box7">
<pose>1.5 0.0375 0.075 0 0 0</pose>
<link name="pyramid_box7_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box7_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box7_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<!-- <model name="pyramid_box8">
<pose>1.5 0.1125 0.075 0 0 0</pose>
<link name="pyramid_box8_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box8_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box8_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<!-- <model name="pyramid_box9">
<pose>1.5 -0.075 0.125 0 0 0</pose>
<link name="pyramid_box9_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box9_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box9_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<model name="pyramid_box10">
<pose>1.5 0 0.125 0 0 0</pose>
<link name="pyramid_box10_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box10_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box10_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<!-- <model name="pyramid_box11">
<pose>1.5 0.075 0.125 0 0 0</pose>
<link name="pyramid_box11_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box11_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box11_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<!-- <model name="pyramid_box12">
<pose>1.5 -0.0375 0.175 0 0 0</pose>
<link name="pyramid_box12_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box12_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box12_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<!-- <model name="pyramid_box13">
<pose>1.5 0.0375 0.175 0 0 0</pose>
<link name="pyramid_box13_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box13_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box13_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
<!-- <model name="pyramid_box14">
<pose>1.5 0 0.225 0 0 0</pose>
<link name="pyramid_box14_link">
<inertial>
<inertia>
<ixx>0.00005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00005</iyy>
<iyz>0.0</iyz>
<izz>0.00005</izz>
</inertia>
<mass>0.12</mass>
</inertial>
<collision name="pyramid_box14_collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="pyramid_box14_visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model> -->
</world>
</sdf>