diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt index 1f913c1a1..d46f84857 100644 --- a/webots_ros2_driver/CMakeLists.txt +++ b/webots_ros2_driver/CMakeLists.txt @@ -109,6 +109,7 @@ add_executable(driver src/plugins/static/Ros2Receiver.cpp src/plugins/static/Ros2Compass.cpp src/plugins/static/Ros2VacuumGripper.cpp + src/plugins/static/Ros2Connector.cpp src/utils/Math.cpp src/utils/Utils.cpp ) diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Connector.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Connector.hpp new file mode 100644 index 000000000..75c6db070 --- /dev/null +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Connector.hpp @@ -0,0 +1,56 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONNECTOR_HPP +#define ROS2_CONNECTOR_HPP + +#include +#include + +#include +#include +#include + +namespace webots_ros2_driver { + class Ros2Connector : public Ros2SensorPlugin { + public: + void step() override; + void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; + + private: + void publishPresence(); + + // ROS2 subscriptions + rclcpp::Subscription::SharedPtr mLockSubscription; + void LockCallback(const std_msgs::msg::Bool::SharedPtr message); + + // ROS2 services + rclcpp::Service::SharedPtr mLockService; + void isLockedCallback(const std::shared_ptr request, + std::shared_ptr response); + + // ROS2 topics + rclcpp::Publisher::SharedPtr mPresencePublisher; + webots_ros2_msgs::msg::IntStamped mPresenceMessage; + bool mIsPresenceEnabled; + + // Device + WbDeviceTag mConnector; + // Runtime vars + std::string mDeviceName; + }; + +} // namespace webots_ros2_driver + +#endif \ No newline at end of file diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp index 62e5f1c71..566fd1d58 100644 --- a/webots_ros2_driver/src/WebotsNode.cpp +++ b/webots_ros2_driver/src/WebotsNode.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -261,6 +262,9 @@ namespace webots_ros2_driver { case WB_NODE_VACUUM_GRIPPER: plugin = std::make_shared(); break; + case WB_NODE_CONNECTOR: + plugin = std::make_shared(); + break; } if (plugin) { plugin->init(this, parameters); diff --git a/webots_ros2_driver/src/plugins/static/Ros2Connector.cpp b/webots_ros2_driver/src/plugins/static/Ros2Connector.cpp new file mode 100644 index 000000000..c84233e68 --- /dev/null +++ b/webots_ros2_driver/src/plugins/static/Ros2Connector.cpp @@ -0,0 +1,83 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "webots_ros2_driver/plugins/static/Ros2Connector.hpp" + +#include +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +namespace webots_ros2_driver { + void Ros2Connector::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { + Ros2SensorPlugin::init(node, parameters); + + // This parameter is read when loading the URDF file + mDeviceName = parameters.count("name") ? parameters["name"] : "connector"; + mConnector = wb_robot_get_device(mDeviceName.c_str()); + + assert(mConnector != 0); + + // Initialize services, publishers and subcriptions + mLockSubscription = mNode->create_subscription( + mTopicName + "/lock", rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2Connector::LockCallback, this, _1)); + + mLockService = mNode->create_service( + mTopicName + "/is_locked", std::bind(&Ros2Connector::isLockedCallback, this, _1, _2)); + + mIsPresenceEnabled = false; + mPresencePublisher = + mNode->create_publisher(mTopicName + "/presence", rclcpp::SensorDataQoS().reliable()); + } + + void Ros2Connector::LockCallback(const std_msgs::msg::Bool::SharedPtr message) { + if (message->data) + wb_connector_lock(mConnector); + else + wb_connector_unlock(mConnector); + } + + void Ros2Connector::isLockedCallback(const std::shared_ptr request, + std::shared_ptr response) { + response->value = wb_connector_is_locked(mConnector); + } + + void Ros2Connector::step() { + if (!preStep()) + return; + + if (mIsPresenceEnabled) + publishPresence(); + + if (mAlwaysOn) + return; + + // Enable/Disable sensor + const bool shouldBeEnabled = mPresencePublisher->get_subscription_count() > 0; + if (shouldBeEnabled != mIsPresenceEnabled) { + if (shouldBeEnabled) + wb_connector_enable_presence(mConnector, mPublishTimestepSyncedMs); + else + wb_connector_disable_presence(mConnector); + mIsPresenceEnabled = shouldBeEnabled; + } + } + + void Ros2Connector::publishPresence() { + mPresenceMessage.header.stamp = mNode->get_clock()->now(); + mPresenceMessage.data = wb_connector_get_presence(mConnector); + mPresencePublisher->publish(mPresenceMessage); + } +} // namespace webots_ros2_driver diff --git a/webots_ros2_msgs/CMakeLists.txt b/webots_ros2_msgs/CMakeLists.txt index cb9b18b12..b08de5cc2 100644 --- a/webots_ros2_msgs/CMakeLists.txt +++ b/webots_ros2_msgs/CMakeLists.txt @@ -26,6 +26,7 @@ set(msg_files "msg/BoolStamped.msg" "msg/FloatStamped.msg" "msg/StringStamped.msg" + "msg/IntStamped.msg" "msg/CameraRecognitionObject.msg" "msg/CameraRecognitionObjects.msg" "msg/UrdfRobot.msg" diff --git a/webots_ros2_msgs/msg/IntStamped.msg b/webots_ros2_msgs/msg/IntStamped.msg new file mode 100644 index 000000000..bc4cf3260 --- /dev/null +++ b/webots_ros2_msgs/msg/IntStamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +int8 data \ No newline at end of file diff --git a/webots_ros2_tests/resource/connector_and_vacuum_gripper_test.urdf b/webots_ros2_tests/resource/connector_and_vacuum_gripper_test.urdf new file mode 100644 index 000000000..b4abaa676 --- /dev/null +++ b/webots_ros2_tests/resource/connector_and_vacuum_gripper_test.urdf @@ -0,0 +1,22 @@ + + + + + + + + + + webots_ros2_control::Ros2ControlSystem + + + + + + + + + + + + diff --git a/webots_ros2_tests/resource/connector_and_vacuum_gripper_test_ros2_control.yml b/webots_ros2_tests/resource/connector_and_vacuum_gripper_test_ros2_control.yml new file mode 100644 index 000000000..f571483dd --- /dev/null +++ b/webots_ros2_tests/resource/connector_and_vacuum_gripper_test_ros2_control.yml @@ -0,0 +1,29 @@ +/robot/controller_manager: + ros__parameters: + update_rate: 50 + + connector_and_vacuum_gripper_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +/robot/connector_and_vacuum_gripper_trajectory_controller: + ros__parameters: + joints: + - connector_motor + - vacuum_gripper_motor + + command_interfaces: + - position + + state_interfaces: + - position + + action_monitor_rate: 20.0 + + allow_partial_joints_goal: false + interpolate_from_desired_state: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 \ No newline at end of file diff --git a/webots_ros2_tests/resource/object_position.urdf.xacro b/webots_ros2_tests/resource/object_position.urdf.xacro new file mode 100644 index 000000000..a238609c8 --- /dev/null +++ b/webots_ros2_tests/resource/object_position.urdf.xacro @@ -0,0 +1,22 @@ + + + + + + + true + true + + + + + + + + webots_ros2_control::Ros2ControlSystem + + + + + + diff --git a/webots_ros2_tests/setup.py b/webots_ros2_tests/setup.py index 1ce29ae7d..f5892a152 100644 --- a/webots_ros2_tests/setup.py +++ b/webots_ros2_tests/setup.py @@ -1,12 +1,28 @@ from setuptools import setup -package_name = 'webots_ros2_tests' +package_name = "webots_ros2_tests" data_files = [ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/worlds', ['worlds/driver_test.wbt', 'worlds/.driver_test.wbproj']), - ('share/' + package_name + '/resource', ['resource/driver_test.urdf']) + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ( + "share/" + package_name + "/worlds", + [ + "worlds/driver_test.wbt", + "worlds/.driver_test.wbproj", + "worlds/connector_and_vacuum_gripper_test.wbt", + "worlds/.connector_and_vacuum_gripper_test.wbproj", + ], + ), + ( + "share/" + package_name + "/resource", + [ + "resource/driver_test.urdf", + "resource/connector_and_vacuum_gripper_test.urdf", + "resource/object_position.urdf.xacro", + "resource/connector_and_vacuum_gripper_test_ros2_control.yml", + ], + ), ] setup( @@ -14,20 +30,20 @@ version='2025.0.1', packages=[package_name], data_files=data_files, - install_requires=['setuptools', 'launch'], + install_requires=["setuptools", "launch"], zip_safe=True, - author='Cyberbotics', - author_email='support@cyberbotics.com', - maintainer='Cyberbotics', - maintainer_email='support@cyberbotics.com', - keywords=['ROS', 'Webots', 'Robot', 'Simulation', 'Examples'], + author="Cyberbotics", + author_email="support@cyberbotics.com", + maintainer="Cyberbotics", + maintainer_email="support@cyberbotics.com", + keywords=["ROS", "Webots", "Robot", "Simulation", "Examples"], classifiers=[ - 'Intended Audience :: Developers', - 'License :: OSI Approved :: Apache Software License', - 'Programming Language :: Python', - 'Topic :: Software Development', + "Intended Audience :: Developers", + "License :: OSI Approved :: Apache Software License", + "Programming Language :: Python", + "Topic :: Software Development", ], - description='System tests for `webots_ros2` packages', - license='Apache License, Version 2.0', - tests_require=['pytest'] + description="System tests for `webots_ros2` packages", + license="Apache License, Version 2.0", + tests_require=["pytest"], ) diff --git a/webots_ros2_tests/test/test_connector_and_vacuum_gripper.py b/webots_ros2_tests/test/test_connector_and_vacuum_gripper.py new file mode 100644 index 000000000..4cf531a12 --- /dev/null +++ b/webots_ros2_tests/test/test_connector_and_vacuum_gripper.py @@ -0,0 +1,339 @@ +#!/usr/bin/env python + +# Copyright 1996-2023 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Test the connector and vacuum gripper.""" + +# Launch the test locally: launch_test src/webots_ros2/webots_ros2_tests/test/test_connector_and_vacuum_gripper.py + +import os +import time +import pytest +import rclpy +from math import isclose +from std_msgs.msg import Bool +from webots_ros2_msgs.msg import IntStamped +from webots_ros2_msgs.srv import GetBool +from sensor_msgs.msg import JointState +from geometry_msgs.msg import PointStamped +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from launch import LaunchDescription +import launch +import launch_testing.actions +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from webots_ros2_driver.webots_launcher import WebotsLauncher +from webots_ros2_driver.webots_controller import WebotsController +from webots_ros2_driver.wait_for_controller_connection import ( + WaitForControllerConnection, +) +from webots_ros2_tests.utils import TestWebots, initialize_webots_test + + +@pytest.mark.rostest +def generate_test_description(): + initialize_webots_test() + + package_dir = get_package_share_directory("webots_ros2_tests") + robot_description_path = os.path.join( + package_dir, "resource", "connector_and_vacuum_gripper_test.urdf" + ) + object_description_path = os.path.join( + package_dir, "resource", "object_position.urdf.xacro" + ) + robot_ros2_control_params = os.path.join( + package_dir, "resource", "connector_and_vacuum_gripper_test_ros2_control.yml" + ) + + webots = WebotsLauncher( + world=os.path.join( + package_dir, "worlds", "connector_and_vacuum_gripper_test.wbt" + ), + ros2_supervisor=True, + ) + + webots_robot_driver = WebotsController( + namespace="robot", + robot_name="connector_and_vacuum_gripper_test_robot", + parameters=[ + { + "robot_description": robot_description_path, + "use_sim_time": True, + "set_robot_state_publisher": True, + }, + robot_ros2_control_params, + ], + respawn=True, + ) + + webots_connector_object_driver = WebotsController( + namespace="connector_object", + robot_name="connector_object", + parameters=[ + { + "robot_description": object_description_path, + "xacro_mappings": ["object_name:='connector_object'"], + "use_sim_time": True, + "set_robot_state_publisher": True, + } + ], + ) + + webots_vacuum_gripper_object_driver = WebotsController( + namespace="vacuum_gripper_object", + robot_name="vacuum_gripper_object", + parameters=[ + { + "robot_description": object_description_path, + "xacro_mappings": ["object_name:='vacuum_gripper_object'"], + "use_sim_time": True, + "set_robot_state_publisher": True, + } + ], + ) + + controller_manager_timeout = ["--controller-manager-timeout", "1000"] + controller_manager_prefix = "python.exe" if os.name == "nt" else "" + robot_position_controller_spawner = Node( + namespace="robot", + package="controller_manager", + executable="spawner", + output="screen", + prefix=controller_manager_prefix, + arguments=[ + "connector_and_vacuum_gripper_trajectory_controller", + "--param-file", + robot_ros2_control_params, + ] + + controller_manager_timeout, + parameters=[ + {"use_sim_time": True}, + ], + ) + robot_joint_state_broadcaster_spawner = Node( + namespace="robot", + package="controller_manager", + executable="spawner", + output="screen", + prefix=controller_manager_prefix, + arguments=[ + "joint_state_broadcaster", + ] + + controller_manager_timeout, + parameters=[ + {"use_sim_time": True}, + ], + ) + + robot_state_publisher = Node( + namespace="robot", + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + {"robot_description": ''}], + ) + + connector_object_state_publisher = Node( + namespace="connector_object", + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + {"robot_description": ''}], + ) + + vacuum_gripper_object_state_publisher = Node( + namespace="vacuum_gripper_object", + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + {"robot_description": ''}], + ) + + robot_ros_control_spawners = [ + robot_position_controller_spawner, + robot_joint_state_broadcaster_spawner, + ] + + robot_waiting_nodes = WaitForControllerConnection( + target_driver=webots_robot_driver, nodes_to_start=robot_ros_control_spawners + ) + + return LaunchDescription( + [ + webots, + webots._supervisor, + robot_state_publisher, + webots_robot_driver, + connector_object_state_publisher, + vacuum_gripper_object_state_publisher, + webots_connector_object_driver, + webots_vacuum_gripper_object_driver, + robot_waiting_nodes, + launch_testing.actions.ReadyToTest(), + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=webots, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + ) + ), + ] + ) + + +class TestConnectorAndVacuumGripper(TestWebots): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.__node = rclpy.create_node("connector_and_vacuum_gripper_tester") + self.joint_publisher = self.__node.create_publisher( + JointTrajectory, + "/robot/connector_and_vacuum_gripper_trajectory_controller/joint_trajectory", + 1, + ) + self.wait_for_clock(self.__node, messages_to_receive=20) + + def wait_for_position(self, index, position): + + def on_position_reached(message): + return isclose(message.position[index], position, rel_tol=1e-3) + + self.wait_for_messages( + self.__node, + JointState, + "/robot/joint_states", + condition=on_position_reached, + ) + + def testConnector(self): + + joint_msg = JointTrajectory() + joint_msg.joint_names = ["connector_motor", "vacuum_gripper_motor"] + + joint_target = JointTrajectoryPoint() + + joint_target.positions = [-0.049, 0.0] + joint_msg.points.append(joint_target) + + time.sleep(5) + + self.joint_publisher.publish(joint_msg) + self.wait_for_position(0, -0.049) + + lock_publisher = self.__node.create_publisher( + Bool, + "/robot/connector_and_vacuum_gripper_test_robot/active_connector/lock", + 1, + ) + + lock_msg = Bool() + lock_msg.data = True + + lock_publisher.publish(lock_msg) + + time.sleep(5) # Give Webots time to connect the object. + + is_locked_client = self.__node.create_client( + GetBool, + "/robot/connector_and_vacuum_gripper_test_robot/active_connector/is_locked", + ) + is_locked_request = GetBool.Request() + is_locked_request.ask = True + + is_locked_future = is_locked_client.call_async(is_locked_request) + rclpy.spin_until_future_complete(self.__node, is_locked_future) + + self.assertTrue(is_locked_future.result().value) + + joint_target.positions = [0.049, 0.0] + joint_msg.points.clear() + joint_msg.points.append(joint_target) + + self.joint_publisher.publish(joint_msg) + self.wait_for_position(0, 0.049) + + def on_message_received(message): + # Object must be connected, fail if not. + self.assertEqual(message.data, 1) + return True + + self.wait_for_messages( + self.__node, + IntStamped, + "/robot/connector_and_vacuum_gripper_test_robot/active_connector/presence", + condition=on_message_received, + ) + + def on_message_received(message): + self.assertAlmostEqual( + message.point.z, 0.15, places=2 + ) # Has object actually been picked up? + return True + + self.wait_for_messages( + self.__node, + PointStamped, + "/connector_object/connector_object/connector_object_position", + condition=on_message_received, + ) + + lock_msg.data = False + + lock_publisher.publish(lock_msg) + + is_locked_future = is_locked_client.call_async(is_locked_request) + rclpy.spin_until_future_complete(self.__node, is_locked_future) + + self.assertFalse(is_locked_future.result().value) + + def on_message_received(message): + self.assertEqual( + message.data, 0 + ) # Object must NOT be connected, fail if not. + return True + + time.sleep(5) + + self.wait_for_messages( + self.__node, + IntStamped, + "/robot/connector_and_vacuum_gripper_test_robot/active_connector/presence", + condition=on_message_received, + ) + + def on_message_received(message): + self.assertAlmostEqual( + message.point.z, 0.05, places=2 + ) # Has object been dropped? + return True + + self.wait_for_messages( + self.__node, + PointStamped, + "/connector_object/connector_object/connector_object_position", + condition=on_message_received, + ) + + def tearDown(self): + self.__node.destroy_node() diff --git a/webots_ros2_tests/worlds/.connector_and_vacuum_gripper_test.wbproj b/webots_ros2_tests/worlds/.connector_and_vacuum_gripper_test.wbproj new file mode 100644 index 000000000..d7d00015c --- /dev/null +++ b/webots_ros2_tests/worlds/.connector_and_vacuum_gripper_test.wbproj @@ -0,0 +1,10 @@ +Webots Project File version R2025a +perspectives: 000000ff00000000fd000000030000000000000069000003ccfc0100000001fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f0077000000000000000069000000000000000000000001000002ad00000234fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002340000003f00ffffff000000030000073e00000191fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000073e0000006900ffffff0000048f0000023400000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000001d20000056a0100000002010000000100 +sceneTreePerspectives: 000000ff00000001000000030000001c000000de000000fa0100000002010000000200 +maximizedDockId: -1 +centralWidgetVisible: 1 +orthographicViewHeight: 1 +textFiles: -1 +globalOptionalRendering: ConnectorAxes +consoles: Console:All:All diff --git a/webots_ros2_tests/worlds/connector_and_vacuum_gripper_test.wbt b/webots_ros2_tests/worlds/connector_and_vacuum_gripper_test.wbt new file mode 100644 index 000000000..c08c87edc --- /dev/null +++ b/webots_ros2_tests/worlds/connector_and_vacuum_gripper_test.wbt @@ -0,0 +1,200 @@ +#VRML_SIM R2025a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/Floor.proto" + +WorldInfo { + info [ + "Test of the connector and vaccuum gripper." + ] + title "ROS2 simulation of connector and vacuum gripper" + gravity 0.1 + basicTimeStep 20 +} +Viewpoint { + orientation 0.06793539504527396 0.21857545744612852 -0.9734523878969419 0.9173500595062111 + position -0.7577203878145741 1.0983589397439193 0.35733599880928457 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +Floor { +} +Robot { + translation 0 0.5 0.05 + children [ + GPS { + name "connector_object_position" + } + Connector { + translation 0 0 0.05 + rotation 0 1 0 -1.5707953071795862 + name "passive_connector" + model "connector" + type "passive" + numberOfRotations 1 + } + Shape { + appearance DEF GREEN Appearance { + material Material { + diffuseColor 0.341176 0.890196 0.537255 + } + } + geometry DEF CYLINDER Cylinder { + height 0.1 + radius 0.05 + } + } + ] + name "connector_object" + boundingObject Cylinder { + height 0.1 + radius 0.05 + } + physics Physics { + density 1 + mass 10 + } + controller "" +} +Robot { + translation 0 -0.5 0.05 + children [ + GPS { + name "vacuum_object_position" + } + Shape { + appearance DEF GREEN Appearance { + material Material { + diffuseColor 0.341176 0.890196 0.537255 + } + } + geometry DEF CYLINDER Cylinder { + height 0.1 + radius 0.05 + } + } + ] + name "vacuum_gripper_object" + boundingObject Cylinder { + height 0.1 + radius 0.05 + } + physics Physics { + density 1 + mass 10 + } + controller "" +} +Robot { + translation 0 0 0.2 + children [ + Pose { + translation 0 0.5 0 + children [ + SliderJoint { + jointParameters JointParameters { + minStop -0.05 + maxStop 0.05 + } + device [ + LinearMotor { + name "connector_motor" + controlPID 1 0 0 + maxVelocity 0.01 + minPosition -0.05 + maxPosition 0.05 + } + PositionSensor { + name "connector_motor_sensor" + } + ] + endPoint Solid { + children [ + Connector { + translation 0 0 -0.05 + rotation 0.7071067811865476 0 -0.7071067811865476 -3.1415853071795863 + name "active_connector" + model "connector" + type "active" + numberOfRotations 1 + } + Shape { + appearance DEF RED Appearance { + material Material { + diffuseColor 0.929412 0.2 0.231373 + } + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + ] + name "connector_gripper" + boundingObject Cylinder { + height 0.1 + radius 0.05 + } + physics Physics { + density 100 + } + } + } + ] + } + Pose { + translation 0 -0.5 0 + children [ + SliderJoint { + jointParameters JointParameters { + minStop -0.05 + maxStop 0.05 + } + device [ + LinearMotor { + name "vacuum_gripper_motor" + controlPID 1 0 0 + maxVelocity 0.01 + minPosition -0.05 + maxPosition 0.05 + } + PositionSensor { + name "vacuum_gripper_motor_sensor" + } + ] + endPoint Solid { + children [ + VacuumGripper { + translation 0 0 -0.05 + rotation 1 0 0 3.141592653589793 + } + Shape { + appearance DEF RED Appearance { + material Material { + diffuseColor 0.929412 0.2 0.231373 + } + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + ] + name "vacuum_gripper" + boundingObject Cylinder { + height 0.1 + radius 0.05 + } + physics Physics { + } + } + } + ] + } + ] + name "connector_and_vacuum_gripper_test_robot" + controller "" +}