diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index 47fdde6..cbf7810 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -44,6 +44,7 @@ jobs: package-name: | flexiv_bringup flexiv_description + flexiv_gripper flexiv_hardware flexiv_moveit_config flexiv_msgs diff --git a/README.md b/README.md index 56a75fb..991c24d 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,12 @@ # Flexiv ROS 2 -[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![docs](https://img.shields.io/badge/docs-sphinx-yellow)](https://rdk.flexiv.com/manual/ros2_packages.html) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![docs](https://img.shields.io/badge/docs-sphinx-yellow)](https://www.flexiv.com/software/rdk/manual/ros2_bridge.html) For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flexiv_rdk), the APIs of RDK are wrapped into ROS packages in `flexiv_ros2`. Key functionalities like real-time joint torque and position control are supported, and the integration with `ros2_control` framework and MoveIt! 2 is also implemented. ## References -[Flexiv RDK main webpage](https://rdk.flexiv.com/) contains important information like RDK user manual and network setup. +[Flexiv RDK main webpage](https://www.flexiv.com/software/rdk) contains important information like RDK user manual and network setup. ## Compatibility @@ -105,14 +105,15 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.0 ## Usage > [!NOTE] -> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://rdk.flexiv.com/manual/ros2_bridge.html) for more information. +> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://www.flexiv.com/software/rdk/manual/ros2_bridge.html) for more information. -The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the robot server](https://rdk.flexiv.com/manual/activate_rdk_server.html) and [establish connection](https://rdk.flexiv.com/manual/establish_connection.html) between the workstation PC and the robot. +The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the robot server](https://www.flexiv.com/software/rdk/manual/activate_rdk_server.html) and [establish connection](https://www.flexiv.com/software/rdk/manual/establish_connection.html) between the workstation PC and the robot. The main launch file to start the robot driver is the `rizon.launch.py` - it loads and starts the robot hardware, joint states broadcaster, Flexiv robot states broadcasters, and robot controller and opens RViZ. The arguments for the launch file are as follows: - `robot_sn` (*required*) - Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456 - `rizon_type` (default: *rizon4*) - type of the Flexiv Rizon robot. (rizon4, rizon4s, rizon10 or rizon10s) +- `load_gripper` (default: *false*) - loads the Flexiv Grav gripper as the end-effector of the robot and the gripper control node. - `use_fake_hardware` (default: *false*) - starts `FakeSystem` instead of real hardware. This is a simple simulation that mimics joint command to their states. - `start_rviz` (deafult: *true*) - starts RViz automatically with the launch file. - `fake_sensor_commands` (default: *false*) - enables fake command interfaces for sensors used for simulations. Used only if `use_fake_hardware` parameter is true. @@ -186,7 +187,7 @@ ros2 launch flexiv_bringup rizon_moveit.launch.py robot_sn:=dont-care use_fake_h The robot driver (`rizon.launch.py`) publishes the following feedback states to the respective ROS topics: -- `/${robot_sn}/flexiv_robot_states`: [Flexiv robot states](https://rdk.flexiv.com/api/structflexiv_1_1rdk_1_1_robot_states.html#details) including the joint- and Cartesian-space robot states. [[`flexiv_msgs/msg/RobotStates.msg`](flexiv_msgs/msg/RobotStates.msg)] +- `/${robot_sn}/flexiv_robot_states`: [Flexiv robot states](https://www.flexiv.com/software/rdk/api/structflexiv_1_1rdk_1_1_robot_states.html) including the joint- and Cartesian-space robot states. [[`flexiv_msgs/msg/RobotStates.msg`](flexiv_msgs/msg/RobotStates.msg)] - `/joint_states`: Measured joint states of the robot: joint position, velocity and torque. [[`sensor_msgs/JointState.msg`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)] - `/flexiv_robot_states_broadcaster/tcp_pose`: Measured TCP pose expressed in world frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)] - `/flexiv_robot_states_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and torque $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)] @@ -201,3 +202,42 @@ The digital output ports on the control box can be set by publishing to the topi ```bash ros2 topic pub /gpio_controller/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}" ``` + +### Gripper Control + +The gripper control is implemented in the `flexiv_gripper` package to interface with the gripper that is connected to the robot. + +Start the `flexiv_gripper_node` with the following launch file: + +```bash +ros2 launch flexiv_gripper flexiv_gripper.launch.py robot_sn:=[robot_sn] +``` + +Or, you can also start the gripper control with the robot driver if the gripper is Flexiv Grav: + +```bash +ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] load_gripper:=true +``` + +#### Gripper Actions + +In a new terminal, send the gripper action `move` goal to open or close the gripper: + +```bash +# Closing the gripper +ros2 action send_goal /flexiv_gripper_node/move flexiv_msgs/action/Move "{width: 0.01, velocity: 0.1, max_force: 20}" +# Opening the gripper +ros2 action send_goal /flexiv_gripper_node/move flexiv_msgs/action/Move "{width: 0.09, velocity: 0.1, max_force: 20}" +``` + +The `grasp` action enables the gripper to grasp with direct force control, but it requires the mounted gripper to support direct force control. Send a `grasp` command to the gripper: + +```bash +ros2 action send_goal /flexiv_gripper_node/grasp flexiv_msgs/action/Grasp "{force: 0}" +``` + +To stop the gripper, send a `stop` service call: + +```bash +ros2 service call /flexiv_gripper_node/stop std_srvs/srv/Trigger {} +``` diff --git a/flexiv_bringup/README.md b/flexiv_bringup/README.md index 57dcdb6..dd22c33 100644 --- a/flexiv_bringup/README.md +++ b/flexiv_bringup/README.md @@ -3,7 +3,7 @@ This package contains launch files: the main driver launcher, the MoveIt launch file and demo examples: - `rizon.launch.py` - the main launcher: starts *ros2_control* node including hardware interface, runs joint states, Flexiv robot states broadcaster, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller. -- `rizon_moveit.launch.py` - runs MoveIt together with the driver. The controller for robot joints started in this launch file is *rizon_arm_controller*. +- `rizon_moveit.launch.py` - runs MoveIt together with the main driver. The controller for robot joints started in this launch file is *rizon_arm_controller*. - `test_joint_trajectory_controller.launch` - sends joint trajectory goals to the *rizon_arm_controller*. - `sine_sweep_position.launch.py` - gets current joint states and then performs a sine-sweep motion with *forward_position_controller*. - `sine_sweep_impedance.launch.py` - gets current joint states and then performs a sine-sweep motion with *joint_impedance_controller*. diff --git a/flexiv_bringup/config/sine_sweep_impedance_config.yaml b/flexiv_bringup/config/sine_sweep_impedance_config.yaml index df5b4c3..9771c3f 100644 --- a/flexiv_bringup/config/sine_sweep_impedance_config.yaml +++ b/flexiv_bringup/config/sine_sweep_impedance_config.yaml @@ -2,5 +2,13 @@ sine_sweep_impedance_controller: ros__parameters: controller_name: "joint_impedance_controller" + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 wait_sec_between_publish: 0.001 speed_scaling: 1.0 diff --git a/flexiv_bringup/config/sine_sweep_position_config.yaml b/flexiv_bringup/config/sine_sweep_position_config.yaml index ad8e1d9..4ba4254 100644 --- a/flexiv_bringup/config/sine_sweep_position_config.yaml +++ b/flexiv_bringup/config/sine_sweep_position_config.yaml @@ -2,5 +2,13 @@ sine_sweep_position_controller: ros__parameters: controller_name: "forward_position_controller" + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 wait_sec_between_publish: 0.001 speed_scaling: 1.0 diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py index b30d8f0..d6e7cac 100644 --- a/flexiv_bringup/launch/rizon.launch.py +++ b/flexiv_bringup/launch/rizon.launch.py @@ -1,21 +1,28 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, +) from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare from launch.substitutions import ( Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, ) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): rizon_type_param_name = "rizon_type" robot_sn_param_name = "robot_sn" start_rviz_param_name = "start_rviz" + load_gripper_param_name = "load_gripper" use_fake_hardware_param_name = "use_fake_hardware" fake_sensor_commands_param_name = "fake_sensor_commands" robot_controller_param_name = "robot_controller" @@ -47,6 +54,14 @@ def generate_launch_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + load_gripper_param_name, + default_value="false", + description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.", + ) + ) + declared_arguments.append( DeclareLaunchArgument( use_fake_hardware_param_name, @@ -76,6 +91,7 @@ def generate_launch_description(): rizon_type = LaunchConfiguration(rizon_type_param_name) robot_sn = LaunchConfiguration(robot_sn_param_name) start_rviz = LaunchConfiguration(start_rviz_param_name) + load_gripper = LaunchConfiguration(load_gripper_param_name) use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name) fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name) robot_controller = LaunchConfiguration(robot_controller_param_name) @@ -86,28 +102,35 @@ def generate_launch_description(): ) # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - flexiv_urdf_xacro, - " ", - "robot_sn:=", - robot_sn, - " ", - "name:=", - "rizon", - " ", - "rizon_type:=", - rizon_type, - " ", - "use_fake_hardware:=", - use_fake_hardware, - " ", - "fake_sensor_commands:=", - fake_sensor_commands, - ] + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + flexiv_urdf_xacro, + " ", + "robot_sn:=", + robot_sn, + " ", + "name:=", + "rizon", + " ", + "rizon_type:=", + rizon_type, + " ", + "load_gripper:=", + load_gripper, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "fake_sensor_commands:=", + fake_sensor_commands, + ] + ), + value_type=str, ) + robot_description = {"robot_description": robot_description_content} # RViZ @@ -134,9 +157,26 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers, {"robot_sn": robot_sn}], + remappings=[("joint_states", "flexiv_arm/joint_states")], output="both", ) + # Joint state publisher + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[ + { + "source_list": [ + "flexiv_arm/joint_states", + "flexiv_gripper_node/gripper_joint_states", + ], + "rate": 30, + } + ], + ) + # Robot state publisher robot_state_publisher_node = Node( package="robot_state_publisher", @@ -173,6 +213,24 @@ def generate_launch_description(): condition=UnlessCondition(use_fake_hardware), ) + # Include gripper launch file + load_gripper_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("flexiv_gripper"), + "launch", + "flexiv_gripper.launch.py", + ] + ) + ), + launch_arguments={ + "robot_sn": robot_sn, + "use_fake_hardware": use_fake_hardware, + }.items(), + condition=IfCondition(load_gripper), + ) + # Run gpio controller gpio_controller_spawner = Node( package="controller_manager", @@ -200,9 +258,11 @@ def generate_launch_description(): nodes = [ ros2_control_node, + joint_state_publisher_node, robot_state_publisher_node, joint_state_broadcaster_spawner, flexiv_robot_states_broadcaster_spawner, + load_gripper_launch, gpio_controller_spawner, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, diff --git a/flexiv_bringup/launch/rizon_moveit.launch.py b/flexiv_bringup/launch/rizon_moveit.launch.py index af038ba..147df83 100644 --- a/flexiv_bringup/launch/rizon_moveit.launch.py +++ b/flexiv_bringup/launch/rizon_moveit.launch.py @@ -3,17 +3,23 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, +) from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare from launch.substitutions import ( Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, ) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def load_yaml(package_name, file_path): @@ -33,6 +39,7 @@ def generate_launch_description(): rizon_type_param_name = "rizon_type" robot_sn_param_name = "robot_sn" start_rviz_param_name = "start_rviz" + load_gripper_param_name = "load_gripper" use_fake_hardware_param_name = "use_fake_hardware" fake_sensor_commands_param_name = "fake_sensor_commands" warehouse_sqlite_path_param_name = "warehouse_sqlite_path" @@ -65,6 +72,14 @@ def generate_launch_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + load_gripper_param_name, + default_value="false", + description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.", + ) + ) + declared_arguments.append( DeclareLaunchArgument( use_fake_hardware_param_name, @@ -101,6 +116,7 @@ def generate_launch_description(): rizon_type = LaunchConfiguration(rizon_type_param_name) robot_sn = LaunchConfiguration(robot_sn_param_name) start_rviz = LaunchConfiguration(start_rviz_param_name) + load_gripper = LaunchConfiguration(load_gripper_param_name) use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name) fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name) warehouse_sqlite_path = LaunchConfiguration(warehouse_sqlite_path_param_name) @@ -111,27 +127,34 @@ def generate_launch_description(): [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] ) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - flexiv_urdf_xacro, - " ", - "robot_sn:=", - robot_sn, - " ", - "name:=", - "rizon", - " ", - "rizon_type:=", - rizon_type, - " ", - "use_fake_hardware:=", - use_fake_hardware, - " ", - "fake_sensor_commands:=", - fake_sensor_commands, - ] + # Get URDF via xacro + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + flexiv_urdf_xacro, + " ", + "robot_sn:=", + robot_sn, + " ", + "name:=", + "rizon", + " ", + "rizon_type:=", + rizon_type, + " ", + "load_gripper:=", + load_gripper, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "fake_sensor_commands:=", + fake_sensor_commands, + ] + ), + value_type=str, ) robot_description = {"robot_description": robot_description_content} @@ -140,17 +163,22 @@ def generate_launch_description(): [FindPackageShare("flexiv_moveit_config"), "srdf", "rizon.srdf.xacro"] ) - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - flexiv_srdf_xacro, - " ", - "name:=", - "rizon", - ] + robot_description_semantic_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + flexiv_srdf_xacro, + " ", + "name:=", + "rizon", + " ", + "load_gripper:=", + load_gripper, + ] + ), + value_type=str, ) - robot_description_semantic = { "robot_description_semantic": robot_description_semantic_content } @@ -268,9 +296,26 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers, {"robot_sn": robot_sn}], + remappings=[("joint_states", "flexiv_arm/joint_states")], output="both", ) + # Joint state publisher + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[ + { + "source_list": [ + "flexiv_arm/joint_states", + "flexiv_gripper_node/gripper_joint_states", + ], + "rate": 30, + } + ], + ) + # Run robot controller robot_controller_spawner = Node( package="controller_manager", @@ -302,6 +347,24 @@ def generate_launch_description(): condition=UnlessCondition(use_fake_hardware), ) + # Include gripper launch file + load_gripper_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("flexiv_gripper"), + "launch", + "flexiv_gripper.launch.py", + ] + ) + ), + launch_arguments={ + "robot_sn": robot_sn, + "use_fake_hardware": use_fake_hardware, + }.items(), + condition=IfCondition(load_gripper), + ) + # Servo node for realtime control servo_yaml = load_yaml( "flexiv_moveit_config", "config/rizon_moveit_servo_config.yaml" @@ -320,6 +383,13 @@ def generate_launch_description(): output="screen", ) + # Run gpio controller + gpio_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["gpio_controller", "--controller-manager", "/controller_manager"], + ) + # Delay rviz start after `joint_state_broadcaster` delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( @@ -340,10 +410,13 @@ def generate_launch_description(): nodes = [ move_group_node, - robot_state_publisher_node, ros2_control_node, + joint_state_publisher_node, + robot_state_publisher_node, joint_state_broadcaster_spawner, flexiv_robot_states_broadcaster_spawner, + load_gripper_launch, + gpio_controller_spawner, servo_node, delay_rviz_after_joint_state_broadcaster_spawner, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, diff --git a/flexiv_bringup/package.xml b/flexiv_bringup/package.xml index 0c932d0..1c708f7 100644 --- a/flexiv_bringup/package.xml +++ b/flexiv_bringup/package.xml @@ -2,7 +2,7 @@ flexiv_bringup - 1.4.0 + 1.5.0 Package with launch files and run-time configurations for Flexiv robots with `ros2_control` Mun Seng Phoon Apache License 2.0 @@ -12,11 +12,13 @@ controller_manager flexiv_description + flexiv_gripper flexiv_hardware flexiv_robot_states_broadcaster flexiv_test_nodes forward_command_controller joint_state_broadcaster + joint_state_publisher joint_state_publisher_gui joint_trajectory_controller robot_state_publisher diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/package.xml b/flexiv_controllers/flexiv_robot_states_broadcaster/package.xml index 49597dd..54ff1fb 100644 --- a/flexiv_controllers/flexiv_robot_states_broadcaster/package.xml +++ b/flexiv_controllers/flexiv_robot_states_broadcaster/package.xml @@ -2,7 +2,7 @@ flexiv_robot_states_broadcaster - 1.4.0 + 1.5.0 The robot states broadcaster publishes the Flexiv robot states. Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_controllers/gpio_controller/package.xml b/flexiv_controllers/gpio_controller/package.xml index 04057cb..3f01c8a 100644 --- a/flexiv_controllers/gpio_controller/package.xml +++ b/flexiv_controllers/gpio_controller/package.xml @@ -2,7 +2,7 @@ gpio_controller - 1.4.0 + 1.5.0 Flexiv custom gpio controller to control the GPIOs of the robot. Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_controllers/joint_impedance_controller/package.xml b/flexiv_controllers/joint_impedance_controller/package.xml index ea0ed67..1616614 100644 --- a/flexiv_controllers/joint_impedance_controller/package.xml +++ b/flexiv_controllers/joint_impedance_controller/package.xml @@ -2,7 +2,7 @@ joint_impedance_controller - 1.4.0 + 1.5.0 The joint impedance controller commands a group of joints in effort interface. Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_description/launch/view_rizon.launch.py b/flexiv_description/launch/view_rizon.launch.py index fd55871..a62584c 100644 --- a/flexiv_description/launch/view_rizon.launch.py +++ b/flexiv_description/launch/view_rizon.launch.py @@ -1,6 +1,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition, UnlessCondition +from launch_ros.parameter_descriptions import ParameterValue from launch.substitutions import ( Command, FindExecutable, @@ -14,23 +15,30 @@ def generate_launch_description(): pkg_share = FindPackageShare("flexiv_description") rizon_type = LaunchConfiguration("rizon_type") + load_gripper = LaunchConfiguration("load_gripper") default_rviz_config_path = PathJoinSubstitution( [pkg_share, "rviz", "view_rizon.rviz"] ) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] - ), - " ", - "name:=", - "rizon", - " ", - "rizon_type:=", - rizon_type, - ] + robot_description_content = ParameterValue( + Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"] + ), + " ", + "name:=", + "rizon", + " ", + "rizon_type:=", + rizon_type, + " ", + "load_gripper:=", + load_gripper, + ] + ), + value_type=str, ) # Robot state publisher @@ -73,6 +81,11 @@ def generate_launch_description(): description="Type of the Flexiv Rizon robot.", choices=["rizon4", "rizon4s", "rizon10", "rizon10s"], ), + DeclareLaunchArgument( + name="load_gripper", + default_value="False", + description="Flag to load the Flexiv Grav gripper", + ), DeclareLaunchArgument( name="gui", default_value="False", diff --git a/flexiv_description/meshes/grav/collision/base.stl b/flexiv_description/meshes/grav/collision/base.stl new file mode 100644 index 0000000..9f49772 Binary files /dev/null and b/flexiv_description/meshes/grav/collision/base.stl differ diff --git a/flexiv_description/meshes/grav/collision/finger_tip.stl b/flexiv_description/meshes/grav/collision/finger_tip.stl new file mode 100644 index 0000000..8c3ab33 Binary files /dev/null and b/flexiv_description/meshes/grav/collision/finger_tip.stl differ diff --git a/flexiv_description/meshes/grav/collision/inner_bar.stl b/flexiv_description/meshes/grav/collision/inner_bar.stl new file mode 100644 index 0000000..899a84e Binary files /dev/null and b/flexiv_description/meshes/grav/collision/inner_bar.stl differ diff --git a/flexiv_description/meshes/grav/collision/outer_bar.stl b/flexiv_description/meshes/grav/collision/outer_bar.stl new file mode 100644 index 0000000..04443fb Binary files /dev/null and b/flexiv_description/meshes/grav/collision/outer_bar.stl differ diff --git a/flexiv_description/meshes/grav/collision/static_assembly.stl b/flexiv_description/meshes/grav/collision/static_assembly.stl new file mode 100644 index 0000000..d578c16 Binary files /dev/null and b/flexiv_description/meshes/grav/collision/static_assembly.stl differ diff --git a/flexiv_description/meshes/grav/visual/base.stl b/flexiv_description/meshes/grav/visual/base.stl new file mode 100644 index 0000000..e94004b Binary files /dev/null and b/flexiv_description/meshes/grav/visual/base.stl differ diff --git a/flexiv_description/meshes/grav/visual/finger_mount.stl b/flexiv_description/meshes/grav/visual/finger_mount.stl new file mode 100644 index 0000000..ce08fc1 Binary files /dev/null and b/flexiv_description/meshes/grav/visual/finger_mount.stl differ diff --git a/flexiv_description/meshes/grav/visual/finger_tip.mtl b/flexiv_description/meshes/grav/visual/finger_tip.mtl new file mode 100644 index 0000000..0d13eda --- /dev/null +++ b/flexiv_description/meshes/grav/visual/finger_tip.mtl @@ -0,0 +1,32 @@ +# Free SolidWorks .OBJ Exporter v2.1 +# num_materials: 3 + +newmtl mat1 +Ka 0.556862745098039 0.556862745098039 0.556862745098039 +Kd 0.556862745098039 0.556862745098039 0.556862745098039 +Ks 0.27843137254902 0.27843137254902 0.27843137254902 +d 1.0 +Tf 1.0 +illum 2 +Ns 599.999994039536 +Ni 1.0 + +newmtl mat2 +Ka 0.898039215686275 0.917647058823529 0.929411764705882 +Kd 0.898039215686275 0.917647058823529 0.929411764705882 +Ks 0.449019607843137 0.458823529411765 0.464705882352941 +d 1.0 +Tf 1.0 +illum 2 +Ns 687.5 +Ni 1.0 + +newmtl mat3 +Ka 0.223529411764706 0.223529411764706 0.223529411764706 +Kd 0.223529411764706 0.223529411764706 0.223529411764706 +Ks 0.111764705882353 0.111764705882353 0.111764705882353 +d 1.0 +Tf 1.0 +illum 2 +Ns 599.999994039536 +Ni 1.0 diff --git a/flexiv_description/meshes/grav/visual/finger_tip.obj b/flexiv_description/meshes/grav/visual/finger_tip.obj new file mode 100644 index 0000000..f4ac740 --- /dev/null +++ b/flexiv_description/meshes/grav/visual/finger_tip.obj @@ -0,0 +1,2996 @@ +# Free SolidWorks .OBJ Exporter v2.1 +# num_vertices: 777 + +mtllib finger_tip.mtl + +v 9.99999999999999 1.19636101791504E-15 19.9999999999999 +v 9.99999999999999 -2.5 19.9999999999999 +v 9.99999999999999 -2.4853448928859 19.72970245394 +v 9.99999999999999 -2.369132927957 19.20174617466 +v 9.99999999999999 -2.14214294041897 18.7111153570574 +v 9.99999999999999 -1.81498872980783 18.2807513528663 +v 9.99999999999999 -1.40296766340596 17.9307775046077 +v 9.99999999999999 -0.925345388349785 17.6775582004579 +v 9.99999999999999 -0.404454991381911 17.5329336936461 +v 9.99999999999999 0.135347271463545 17.5036664653721 +v 9.99999999999999 0.668820846323053 17.5911250187019 +v 9.99999999999999 1.17102110174948 17.7912198888849 +v 9.99999999999999 1.61846571195457 18.0945948621808 +v 9.99999999999999 1.99023266426411 18.4870644620155 +v 9.99999999999999 2.26893854917739 18.9502772460992 +v 9.99999999999999 2.44155138927522 19.4625738994723 +v 9.99999999999999 2.5 19.9999999999999 +v 9.99999999999999 2.44155138927522 20.5374261005275 +v 9.99999999999999 2.26893854917739 21.0497227539006 +v 9.99999999999999 1.99023266426411 21.5129355379843 +v 9.99999999999999 1.61846571195457 21.905405137819 +v 9.99999999999999 1.17102110174948 22.208780111115 +v 9.99999999999999 0.668820846323053 22.408874981298 +v 9.99999999999999 0.135347271463545 22.4963335346277 +v 9.99999999999999 -0.40445499138191 22.4670663063537 +v 9.99999999999999 -0.925345388349785 22.3224417995419 +v 9.99999999999999 -1.40296766340595 22.0692224953921 +v 9.99999999999999 -1.81498872980782 21.7192486471335 +v 9.99999999999999 -2.14214294041897 21.2888846429425 +v 9.99999999999999 -2.369132927957 20.7982538253399 +v 9.99999999999999 -2.4853448928859 20.2702975460598 +v 9.99999999999999 -0.925345388349785 22.3224417995419 +v 9.99999999999999 -0.40445499138191 22.4670663063537 +v 6.26468647543159 -0.404454991381911 22.4670663063537 +v 9.99999999999999 -1.40296766340595 22.0692224953921 +v 6.25457334474134 -0.925345388349787 22.3224417995419 +v 9.99999999999999 -1.81498872980782 21.7192486471335 +v 6.23686652607959 -1.40296766340595 22.0692224953921 +v 9.99999999999999 -2.14214294041897 21.2888846429425 +v 6.21239397060726 -1.81498872980783 21.7192486471335 +v 9.99999999999999 -2.369132927957 20.7982538253399 +v 6.18229998781895 -2.14214294041897 21.2888846429425 +v 9.99999999999999 -2.4853448928859 20.2702975460598 +v 6.14799173890276 -2.36913292795701 20.7982538253399 +v 9.99999999999999 -2.5 19.9999999999999 +v 6.11107343944715 -2.4853448928859 20.2702975460598 +v 6.07327134810292 -2.4853448928859 19.72970245394 +v 9.99999999999999 -2.4853448928859 19.72970245394 +v 9.99999999999999 -2.369132927957 19.20174617466 +v 9.99999999999999 -2.14214294041897 18.7111153570574 +v 6.0363530486473 -2.369132927957 19.20174617466 +v 9.99999999999999 -1.81498872980783 18.2807513528663 +v 6.00204479973112 -2.14214294041897 18.7111153570574 +v 9.99999999999999 -1.40296766340596 17.9307775046077 +v 5.9719508169428 -1.81498872980783 18.2807513528663 +v 9.99999999999999 -0.925345388349785 17.6775582004579 +v 5.94747826147047 -1.40296766340596 17.9307775046077 +v 9.99999999999999 -0.404454991381911 17.5329336936461 +v 5.92977144280873 -0.925345388349785 17.6775582004579 +v 9.99999999999999 0.135347271463545 17.5036664653721 +v 5.91965831211848 -0.404454991381911 17.5329336936461 +v 5.91761174815085 0.135347271463542 17.5036664653721 +v 5.92372744596239 0.668820846323053 17.5911250187019 +v 9.99999999999999 0.668820846323053 17.5911250187019 +v 5.93771944232054 1.17102110174948 17.7912198888849 +v 9.99999999999999 1.17102110174948 17.7912198888849 +v 5.95893348702657 1.61846571195457 18.0945948621808 +v 9.99999999999999 1.61846571195457 18.0945948621808 +v 5.98637763492776 1.99023266426411 18.4870644620155 +v 9.99999999999999 1.99023266426411 18.4870644620155 +v 6.01876862817021 2.26893854917739 18.9502772460992 +v 9.99999999999999 2.26893854917739 18.9502772460992 +v 6.05459189990991 2.44155138927522 19.4625738994723 +v 9.99999999999999 2.44155138927522 19.4625738994723 +v 6.09217239377503 2.5 19.9999999999999 +v 9.99999999999999 2.5 19.9999999999999 +v 6.12975288764015 2.44155138927522 20.5374261005275 +v 9.99999999999999 2.44155138927522 20.5374261005275 +v 6.16557615937986 2.26893854917739 21.0497227539006 +v 9.99999999999999 2.26893854917739 21.0497227539006 +v 6.19796715262231 1.99023266426411 21.5129355379843 +v 9.99999999999999 1.99023266426411 21.5129355379843 +v 6.22541130052349 1.61846571195457 21.905405137819 +v 9.99999999999999 1.61846571195457 21.905405137819 +v 6.24662534522953 1.17102110174948 22.208780111115 +v 9.99999999999999 1.17102110174948 22.208780111115 +v 6.26061734158767 0.668820846323055 22.408874981298 +v 9.99999999999999 0.668820846323053 22.408874981298 +v 6.26571875266492 0.133371043663252 22.481828558552 +v 9.99999999999999 0.135347271463545 22.4963335346277 +v 6.26698942363379 9.0283903450555E-16 22.4999999999999 +v 5.91965831211848 -0.404454991381911 17.5329336936461 +v 5.93771944232054 1.17102110174948 17.7912198888849 +v 5.92372744596239 0.668820846323053 17.5911250187019 +v 6.26571875266492 0.133371043663252 22.481828558552 +v 6.26061734158767 0.668820846323055 22.408874981298 +v 6.26468647543159 -0.404454991381911 22.4670663063537 +v 6.21239397060726 -1.81498872980783 21.7192486471335 +v 6.23686652607959 -1.40296766340595 22.0692224953921 +v 5.92977144280873 -0.925345388349785 17.6775582004579 +v 5.95893348702657 1.61846571195457 18.0945948621808 +v 6.24662534522953 1.17102110174948 22.208780111115 +v 6.18229998781895 -2.14214294041897 21.2888846429425 +v 5.94747826147047 -1.40296766340596 17.9307775046077 +v 5.98637763492776 1.99023266426411 18.4870644620155 +v 6.22541130052349 1.61846571195457 21.905405137819 +v 6.14799173890276 -2.36913292795701 20.7982538253399 +v 5.9719508169428 -1.81498872980783 18.2807513528663 +v 6.01876862817021 2.26893854917739 18.9502772460992 +v 6.19796715262231 1.99023266426411 21.5129355379843 +v 6.11107343944715 -2.4853448928859 20.2702975460598 +v 6.00204479973112 -2.14214294041897 18.7111153570574 +v 6.05459189990991 2.44155138927522 19.4625738994723 +v 6.16557615937986 2.26893854917739 21.0497227539006 +v 6.07327134810292 -2.4853448928859 19.72970245394 +v 6.0363530486473 -2.369132927957 19.20174617466 +v 6.09217239377503 2.5 19.9999999999999 +v 6.12975288764015 2.44155138927522 20.5374261005275 +v 6.26698942363379 9.0283903450555E-16 22.4999999999999 +v 6.25457334474134 -0.925345388349787 22.3224417995419 +v 5.91761174815085 0.135347271463542 17.5036664653721 +v 4.5 6.25 35.5 +v 5 6.25 36 +v 5 6.75 35.5 +v 4.5 6.25 35.5 +v 4.80865828381745 6.71193976625564 15.6595881654721 +v 4.58426519384873 6.5277851165098 15.5076020842588 +v 5 6.75 35.5 +v 5 6.75 15.7891878675809 +v 4.5 6.25 15.4505275255122 +v 4.5 -6.25 35.5 +v 4.5 6.25 35.5 +v 4.5 6.25 15.4505275255122 +v 4.5 -6.25 15.4505275255122 +v 4.5 6.25 15.4505275255122 +v 4.58426519384873 6.5277851165098 15.5076020842588 +v 4.80865828381745 6.71193976625564 15.6595881654721 +v 5.83738467214089 -6.75 16.3563658266016 +v 4.5 -6.25 15.4505275255122 +v 5 -6.75 15.7891878675809 +v 4.7222148834902 -6.66573480615128 15.6010382624233 +v 5 6.75 15.7891878675809 +v 4.53806023374436 -6.44134171618255 15.4763065090704 +v 5.83738467214089 6.75 16.3563658266016 +v 5.83738467214089 -6.75 16.3563658266016 +v 5.83738467214089 6.75 16.3563658266016 +v 6.96625754306883 6.75 32.5 +v 6.96625754306883 -6.75 32.5 +v 9.99999999999999 -6.75 32.5 +v 6.96625754306883 -6.75 32.5 +v 6.96625754306883 6.75 32.5 +v 9.99999999999999 6.75 32.5 +v 9.99999999999999 6.75 32.5 +v 9.99999999999999 6.75 3 +v 9.99999999999999 -6.75 3 +v 9.99999999999999 -6.75 32.5 +v 9.99999999999999 -6.75 3 +v 9.99999999999999 6.75 3 +v 8.49999999999999 6.75 3 +v 8.49999999999999 -6.75 3 +v 8.49999999999999 -6.75 3 +v 8.49999999999999 6.75 3 +v 8.49999999999999 6.75 5 +v 8.49999999999999 -6.75 5 +v 8.49999999999999 -6.75 5 +v 8.49999999999999 6.75 5 +v 5.49999999999999 6.75 5 +v 5.49999999999999 -6.75 5 +v 5.49999999999999 6.75 5 +v 5.49999999999999 6.75 0 +v 5.49999999999999 -6.75 0 +v 5.49999999999999 -6.75 5 +v 5 6.75 15.7891878675809 +v 5 6.75 35.5 +v 6.96625754306883 6.75 32.5 +v 13 6.75 35.5 +v 9.99999999999999 6.75 32.5 +v 13 6.75 8.13151629364128E-16 +v 9.99999999999999 6.75 3 +v 5.49999999999999 6.75 0 +v 8.49999999999999 6.75 3 +v 5.49999999999999 6.75 5 +v 8.49999999999999 6.75 5 +v 5.83738467214089 6.75 16.3563658266016 +v 4.5 -6.25 35.5 +v 4.7222148834902 -6.66573480615128 15.6010382624233 +v 5 -6.75 15.7891878675809 +v 4.5 -6.25 15.4505275255122 +v 4.53806023374436 -6.44134171618255 15.4763065090704 +v 5 -6.75 35.5 +v 5 -6.25 36 +v 5 6.25 36 +v 4.5 6.25 35.5 +v 4.5 -6.25 35.5 +v 13 6.75 35.5 +v 5 6.75 35.5 +v 5 6.25 36 +v 13 6.25 36 +v 13 6.75 8.13151629364128E-16 +v 13 6.75 35.5 +v 13.5 6.25 35.5 +v 13.5 6.25 8.67361737988404E-16 +v 13 -6.75 8.13151629364128E-16 +v 13 6.75 8.13151629364128E-16 +v 13.5 6.25 8.67361737988404E-16 +v 5.49999999999999 6.75 0 +v 13.5 -6.25 8.67361737988404E-16 +v 5.49999999999999 -6.75 0 +v 13 -6.75 8.13151629364128E-16 +v 9.99999999999999 -6.75 3 +v 8.49999999999999 -6.75 3 +v 13 -6.75 35.5 +v 9.99999999999999 -6.75 32.5 +v 5 -6.75 35.5 +v 6.96625754306883 -6.75 32.5 +v 5 -6.75 15.7891878675809 +v 5.83738467214089 -6.75 16.3563658266016 +v 5.49999999999999 -6.75 5 +v 5.49999999999999 -6.75 0 +v 8.49999999999999 -6.75 5 +v 5 -6.75 35.5 +v 5 -6.25 36 +v 4.5 -6.25 35.5 +v 13 6.25 36 +v 5 6.25 36 +v 5 -6.25 36 +v 13 -6.25 36 +v 13.5 -6.25 35.5 +v 13.5 -6.25 8.67361737988404E-16 +v 13.5 6.25 8.67361737988404E-16 +v 13.5 6.25 35.5 +v 13 6.25 36 +v 13.5 6.25 35.5 +v 13 6.75 35.5 +v 13 -6.75 35.5 +v 13 -6.75 8.13151629364128E-16 +v 13.5 -6.25 8.67361737988404E-16 +v 13.5 -6.25 35.5 +v 5 -6.75 35.5 +v 13 -6.75 35.5 +v 13 -6.25 36 +v 5 -6.25 36 +v 13.5 -6.25 35.5 +v 13.5 6.25 35.5 +v 13 6.25 36 +v 13 -6.25 36 +v 13 -6.75 35.5 +v 13.5 -6.25 35.5 +v 13 -6.25 36 +v -3 4 4.60086061902756 +v -3.00000000000001 4.99999999999999 4 +v -2.56698729810779 4.74999999999999 4 +v -3 4 4.60086061902756 +v -2.13397459621558 4.5 4 +v -2.13397459621558 4 4 +v -3 4 4.60086061902756 +v -2.13397459621558 3.50000000000001 4 +v -2.56698729810779 3.25000000000001 4 +v -3 4 4.60086061902756 +v -3 3.00000000000001 4 +v -3.43301270189222 3.25000000000001 4 +v -3 4 4.60086061902756 +v -3.86602540378443 3.50000000000001 4 +v -3.86602540378443 4 4 +v -3 4 4.60086061902756 +v -3.86602540378443 4.49999999999999 4 +v -3.43301270189222 4.74999999999999 4 +v -3 4 4.60086061902756 +v -3 4 4.60086061902756 +v -3 4 4.60086061902756 +v -3 4 4.60086061902756 +v -3 4 4.60086061902756 +v -3 4 4.60086061902756 +v -3.5 4.86602540378443 -1.4328367550024E-15 +v -3.00000000000001 4.99999999999999 -1.4328367550024E-15 +v -3.00000000000001 4.99999999999999 4 +v -3.86602540378443 4.49999999999999 -1.4328367550024E-15 +v -3.43301270189222 4.74999999999999 4 +v -3.99999999999999 4 -1.4328367550024E-15 +v -3.86602540378443 4.49999999999999 4 +v -3.86602540378443 3.50000000000001 -1.4328367550024E-15 +v -3.86602540378443 4 4 +v -3.5 3.13397459621557 -1.4328367550024E-15 +v -3.86602540378443 3.50000000000001 4 +v -3 3.00000000000001 -1.4328367550024E-15 +v -3.43301270189222 3.25000000000001 4 +v -2.50000000000001 3.13397459621557 -1.4328367550024E-15 +v -3 3.00000000000001 4 +v -2.13397459621558 3.50000000000001 -1.4328367550024E-15 +v -2.56698729810779 3.25000000000001 4 +v -2.00000000000002 4 -1.4328367550024E-15 +v -2.13397459621558 3.50000000000001 4 +v -2.13397459621558 4.5 -1.4328367550024E-15 +v -2.13397459621558 4 4 +v -2.50000000000001 4.86602540378443 -1.4328367550024E-15 +v -2.13397459621558 4.5 4 +v -2.56698729810779 4.74999999999999 4 +v 5.49999999999999 -6.75 0 +v -2.13397459621557 -4.49999999999999 -1.4328367550024E-15 +v 0 -2.25 0 +v -2.00000000000001 -4 -1.4328367550024E-15 +v 1.125 -1.94855715851499 0 +v 1.94855715851499 -1.125 0 +v -2.50000000000001 -4.86602540378443 -1.4328367550024E-15 +v -1.94855715851499 -1.125 0 +v -1.125 -1.94855715851498 0 +v -2.25 2.14253851000663E-15 0 +v -4.5 -7.25 0 +v -3 -4.99999999999999 -1.4328367550024E-15 +v -3.5 -4.86602540378443 -1.4328367550024E-15 +v -3.99999999999999 -4 -1.4328367550024E-15 +v -3.86602540378443 -4.5 -1.4328367550024E-15 +v -4.50000000000001 7.25 1.020425574104E-16 +v -3.86602540378443 -3.50000000000001 -1.4328367550024E-15 +v -3.5 -3.13397459621557 -1.4328367550024E-15 +v -3.99999999999999 4 -1.4328367550024E-15 +v -3.86602540378443 4.49999999999999 -1.4328367550024E-15 +v -3.86602540378443 3.50000000000001 -1.4328367550024E-15 +v -2.13397459621557 -3.50000000000001 -1.4328367550024E-15 +v 8.67361737988404E-16 2.25 0 +v -1.125 1.94855715851499 0 +v -2.13397459621558 3.50000000000001 -1.4328367550024E-15 +v 5.49999999999999 6.75 0 +v 1.125 1.94855715851499 0 +v -3 -3.00000000000001 -1.4328367550024E-15 +v -1.94855715851498 1.125 0 +v -3.5 3.13397459621557 -1.4328367550024E-15 +v -2.50000000000001 3.13397459621557 -1.4328367550024E-15 +v -2.00000000000002 4 -1.4328367550024E-15 +v 2.25 -3.68191896954514E-16 0 +v -2.50000000000001 -3.13397459621557 -1.4328367550024E-15 +v -3 3.00000000000001 -1.4328367550024E-15 +v 1.94855715851499 1.125 0 +v -2.50000000000001 4.86602540378443 -1.4328367550024E-15 +v 9.99999999999999 7.25 8.41851098635803E-16 +v 10.5 6.75 -7.63844010796873E-17 +v -3.00000000000001 4.99999999999999 -1.4328367550024E-15 +v -3.5 4.86602540378443 -1.4328367550024E-15 +v -2.13397459621558 4.5 -1.4328367550024E-15 +v 9.99999999999999 -7.25 8.41851098635803E-16 +v 10.5 -6.75 -7.63844010796873E-17 +v -4.50000000000001 7.25 1.020425574104E-16 +v -4.5 -7.25 0 +v -4.5 -7.25 -1.8 +v -4.5 7.25 -1.8 +v -4.5 -7.25 -1.8 +v -4.70000000000001 -7.25 -2 +v -4.70000000000001 7.25 -2 +v -4.5 7.25 -1.8 +v 5.49999999999999 6.75 5 +v 5.49999999999999 -6.75 5 +v 5.49999999999999 -6.75 0 +v 5.49999999999999 6.75 0 +v 8.49999999999999 -6.75 5 +v 5.49999999999999 -6.75 5 +v 5.49999999999999 6.75 5 +v 8.49999999999999 6.75 5 +v 8.49999999999999 -6.75 3 +v 8.49999999999999 -6.75 5 +v 8.49999999999999 6.75 5 +v 8.49999999999999 6.75 3 +v 9.99999999999999 -6.75 3 +v 8.49999999999999 -6.75 3 +v 8.49999999999999 6.75 3 +v 9.99999999999999 6.75 3 +v 10.5 -6.75 -7.63844010796873E-17 +v 10.5 -6.75 33 +v 9.99999999999999 -7.25 32.5 +v 9.99999999999999 -7.25 8.41851098635803E-16 +v 6.5 -6.75 33 +v 6.96625754306883 -7.25 32.5 +v 9.99999999999999 -7.25 32.5 +v 10.5 -6.75 33 +v 5.37912260486461 -6.75 16.9707064574746 +v 5.87790462999453 -7.25 16.9358282206026 +v 6.96625754306883 -7.25 32.5 +v 6.5 -6.75 33 +v 4.78597486949456 -7.25 15.0403278484472 +v 5.55026343831534 -7.25 15.8623095902267 +v 5.11700965152126 -6.75 16.1118915531739 +v 5.87790462999453 -7.25 16.9358282206026 +v 5.37912260486461 -6.75 16.9707064574746 +v 4.50557879646464 -6.75 15.4543061597503 +v 10.5 6.75 -7.63844010796873E-17 +v 9.99999999999999 7.25 8.41851098635803E-16 +v 9.99999999999999 7.25 32.5 +v 10.5 6.75 33 +v 6.5 6.75 33 +v 10.5 6.75 33 +v 9.99999999999999 7.25 32.5 +v 6.96625754306882 7.25 32.5 +v 5.37912260486461 6.75 16.9707064574746 +v 6.5 6.75 33 +v 6.96625754306882 7.25 32.5 +v 5.87790462999452 7.25 16.9358282206026 +v 5.87790462999452 7.25 16.9358282206026 +v 5.55026343831533 7.25 15.8623095902267 +v 5.11700965152127 6.75000000000001 16.1118915531739 +v 4.78597486949455 7.25 15.0403278484472 +v 4.50557879646464 6.75 15.4543061597503 +v 5.37912260486461 6.75 16.9707064574746 +v -1.125 -1.94855715851498 6.40000000000002 +v -8.67361737988404E-16 -2.25 6.40000000000002 +v 0 -2.25 0 +v -1.94855715851499 -1.125 6.40000000000002 +v -1.125 -1.94855715851498 0 +v -2.25 2.14253851000663E-15 6.40000000000002 +v -1.94855715851499 -1.125 0 +v -1.94855715851498 1.125 6.40000000000002 +v -2.25 2.14253851000663E-15 0 +v -1.125 1.94855715851499 6.40000000000002 +v -1.94855715851498 1.125 0 +v 8.67361737988404E-16 2.25 6.40000000000002 +v -1.125 1.94855715851499 0 +v 1.125 1.94855715851499 6.40000000000002 +v 8.67361737988404E-16 2.25 0 +v 1.94855715851499 1.125 6.40000000000002 +v 1.125 1.94855715851499 0 +v 2.25 -3.68191896954514E-16 6.40000000000002 +v 1.94855715851499 1.125 0 +v 1.94855715851499 -1.125 6.40000000000002 +v 2.25 -3.68191896954514E-16 0 +v 1.125 -1.94855715851499 6.40000000000002 +v 1.94855715851499 -1.125 0 +v 1.125 -1.94855715851499 0 +v 3.69551813004515 -1.53073372946036 6.40000000000002 +v 4.00000000000001 1.12179346703667E-15 6.40000000000002 +v 2.25 -3.68191896954514E-16 6.40000000000002 +v 3.69551813004515 1.53073372946036 6.40000000000002 +v 1.94855715851499 1.125 6.40000000000002 +v 2.82842712474619 2.8284271247462 6.40000000000002 +v 1.125 1.94855715851499 6.40000000000002 +v 1.53073372946036 3.69551813004515 6.40000000000002 +v 0 4.00000000000001 6.40000000000002 +v 8.67361737988404E-16 2.25 6.40000000000002 +v -1.53073372946036 3.69551813004515 6.40000000000002 +v -1.125 1.94855715851499 6.40000000000002 +v -2.82842712474619 2.82842712474619 6.40000000000002 +v -1.94855715851498 1.125 6.40000000000002 +v -3.69551813004515 1.53073372946036 6.40000000000002 +v -4.00000000000001 2.56243672322396E-16 6.40000000000002 +v -2.25 2.14253851000663E-15 6.40000000000002 +v 2.82842712474619 -2.82842712474619 6.40000000000002 +v 1.94855715851499 -1.125 6.40000000000002 +v 1.53073372946036 -3.69551813004515 6.40000000000002 +v 1.125 -1.94855715851499 6.40000000000002 +v -8.67361737988404E-16 -2.25 6.40000000000002 +v -3.69551813004515 -1.53073372946036 6.40000000000002 +v -1.94855715851499 -1.125 6.40000000000002 +v -2.82842712474619 -2.82842712474619 6.40000000000002 +v -1.125 -1.94855715851498 6.40000000000002 +v -1.53073372946036 -3.69551813004515 6.40000000000002 +v 8.67361737988404E-16 -4.00000000000001 6.40000000000002 +v 4.00000000000001 1.12179346703667E-15 6.40000000000002 +v 3.92314112161293 0.780361288064516 15.0598090753523 +v 3.69551813004515 1.53073372946036 14.9056353149782 +v 4.00000000000001 2.33615047336546E-16 15.1118671834435 +v 3.69551813004515 1.53073372946036 6.40000000000002 +v 3.32587844921019 2.22228093207841 14.6552707134707 +v 2.82842712474619 2.8284271247462 14.3183366420597 +v 2.82842712474619 2.8284271247462 6.40000000000002 +v 2.22228093207841 3.32587844921019 13.9077812881546 +v 1.53073372946036 3.69551813004515 13.439382063764 +v 1.53073372946036 3.69551813004515 6.40000000000002 +v 0.780361288064513 3.92314112161293 12.9311392883998 +v 0 4.00000000000001 12.4025844468936 +v 0 4.00000000000001 6.40000000000002 +v -0.780361288064515 3.92314112161293 11.8740296053873 +v -1.53073372946036 3.69551813004515 11.3657868300232 +v -1.53073372946036 3.69551813004515 6.40000000000002 +v -2.22228093207841 3.32587844921019 10.8973876056326 +v -2.82842712474619 2.82842712474619 10.4868322517275 +v -2.82842712474619 2.82842712474619 6.40000000000002 +v -3.32587844921019 2.22228093207841 10.1498981803165 +v -3.69551813004515 1.53073372946036 9.89953357880896 +v -4.00000000000001 2.56243672322396E-16 6.40000000000002 +v -3.69551813004515 1.53073372946036 6.40000000000002 +v -3.92314112161293 0.780361288064512 9.74535981843486 +v -4.00000000000001 2.56243672322396E-16 9.69330171034368 +v -3.92314112161293 -0.780361288064515 9.74535981843486 +v -3.69551813004515 -1.53073372946036 6.40000000000002 +v -2.82842712474619 -2.82842712474619 6.40000000000002 +v -3.69551813004515 -1.53073372946036 9.89953357880896 +v -3.32587844921019 -2.22228093207841 10.1498981803165 +v -1.53073372946036 -3.69551813004515 6.40000000000002 +v -2.82842712474619 -2.82842712474619 10.4868322517275 +v -2.22228093207841 -3.32587844921019 10.8973876056326 +v 8.67361737988404E-16 -4.00000000000001 6.40000000000002 +v -1.53073372946036 -3.69551813004515 11.3657868300232 +v -0.780361288064515 -3.92314112161293 11.8740296053873 +v 1.53073372946036 -3.69551813004515 6.40000000000002 +v 8.67361737988404E-16 -4.00000000000001 12.4025844468936 +v 0.780361288064515 -3.92314112161293 12.9311392883998 +v 2.82842712474619 -2.82842712474619 6.40000000000002 +v 1.53073372946036 -3.69551813004515 13.439382063764 +v 2.22228093207841 -3.32587844921019 13.9077812881546 +v 3.69551813004515 -1.53073372946036 6.40000000000002 +v 2.82842712474619 -2.82842712474619 14.3183366420597 +v 3.32587844921019 -2.22228093207841 14.6552707134707 +v 3.69551813004515 -1.53073372946036 14.9056353149782 +v 3.92314112161293 -0.780361288064513 15.0598090753523 +v -5.62158429211968 -6.75 8.5949691282186 +v -5.34118821908976 -7.25 8.18099081691547 +v 4.78597486949456 -7.25 15.0403278484472 +v 4.50557879646464 -6.75 15.4543061597503 +v 9.99999999999999 -7.25 8.41851098635803E-16 +v -5.34118821908976 -7.25 8.18099081691547 +v -5.8251004355585 -7.25 7.641984638772 +v -6.00000000000001 -7.25 6.93905588300604 +v 4.78597486949456 -7.25 15.0403278484472 +v 9.99999999999999 -7.25 32.5 +v 6.96625754306883 -7.25 32.5 +v 5.87790462999453 -7.25 16.9358282206026 +v 5.55026343831534 -7.25 15.8623095902267 +v -4.5 -7.25 0 +v -6.00000000000001 -7.25 -2 +v -4.70000000000001 -7.25 -2 +v -4.5 -7.25 -1.8 +v -6.50000000000001 -6.75 -2 +v -6.00000000000001 -7.25 -2 +v -6.00000000000001 -7.25 6.93905588300604 +v -6.50000000000001 -6.75 6.93905588300604 +v -4.70000000000001 7.25 -2 +v -4.70000000000001 -7.25 -2 +v -6.50000000000001 -6.75 -2 +v -6.50000000000001 6.75 -2 +v -6.00000000000001 -7.25 -2 +v -6.00000000000001 7.25 -2 +v 9.99999999999999 7.25 8.41851098635803E-16 +v -6.00000000000001 7.25 6.93905588300604 +v -5.82510043555851 7.25 7.641984638772 +v -4.50000000000001 7.25 1.020425574104E-16 +v 9.99999999999999 7.25 32.5 +v 5.87790462999452 7.25 16.9358282206026 +v -5.34118821908976 7.25 8.18099081691546 +v -4.5 7.25 -1.8 +v -4.70000000000001 7.25 -2 +v 5.55026343831533 7.25 15.8623095902267 +v 4.78597486949455 7.25 15.0403278484472 +v -6.00000000000001 7.25 -2 +v 6.96625754306882 7.25 32.5 +v -6.00000000000001 7.25 6.93905588300604 +v -6.00000000000001 7.25 -2 +v -6.50000000000001 6.75 -2 +v -6.50000000000001 6.75000000000001 6.93905588300604 +v 4.78597486949455 7.25 15.0403278484472 +v -5.34118821908976 7.25 8.18099081691546 +v -5.62158429211968 6.75000000000001 8.5949691282186 +v 4.50557879646464 6.75 15.4543061597503 +v -6.50000000000001 -6.75 -2 +v -6.50000000000001 -6.75 6.93905588300604 +v -6.50000000000001 6.75000000000001 6.93905588300604 +v -6.50000000000001 6.75 -2 +v -6.50000000000001 6.75000000000001 6.93905588300604 +v -6.50000000000001 -6.75 6.93905588300604 +v -6.26680058074467 -6.75 7.87629422402732 +v -6.26680058074467 6.75000000000001 7.87629422402732 +v -5.62158429211968 -6.75 8.5949691282186 +v -5.62158429211968 6.75000000000001 8.5949691282186 +v -6.50000000000001 -6.75 6.93905588300604 +v -6.00000000000001 -7.25 6.93905588300604 +v -5.8251004355585 -7.25 7.641984638772 +v -6.26680058074467 -6.75 7.87629422402732 +v -5.34118821908976 -7.25 8.18099081691547 +v -5.62158429211968 -6.75 8.5949691282186 +v -5.62158429211968 6.75000000000001 8.5949691282186 +v -5.34118821908976 7.25 8.18099081691546 +v -5.82510043555851 7.25 7.641984638772 +v -6.26680058074467 6.75000000000001 7.87629422402732 +v -6.00000000000001 7.25 6.93905588300604 +v -6.50000000000001 6.75000000000001 6.93905588300604 +v -5.62158429211968 -6.75 8.5949691282186 +v 4.50557879646464 -6.75 15.4543061597503 +v 8.67361737988404E-16 -4.00000000000001 12.4025844468936 +v -5.62158429211968 6.75000000000001 8.5949691282186 +v -0.780361288064515 3.92314112161293 11.8740296053873 +v 0 4.00000000000001 12.4025844468936 +v 0.780361288064515 -3.92314112161293 12.9311392883998 +v 4.50557879646464 6.75 15.4543061597503 +v 1.53073372946036 -3.69551813004515 13.439382063764 +v 0.780361288064513 3.92314112161293 12.9311392883998 +v -0.780361288064515 -3.92314112161293 11.8740296053873 +v -1.53073372946036 3.69551813004515 11.3657868300232 +v -1.53073372946036 -3.69551813004515 11.3657868300232 +v 1.53073372946036 3.69551813004515 13.439382063764 +v 2.22228093207841 -3.32587844921019 13.9077812881546 +v 2.82842712474619 -2.82842712474619 14.3183366420597 +v -2.22228093207841 3.32587844921019 10.8973876056326 +v 5.83738467214089 -6.75 16.3563658266016 +v 2.82842712474619 2.8284271247462 14.3183366420597 +v 5.83738467214089 6.75 16.3563658266016 +v -2.82842712474619 2.82842712474619 10.4868322517275 +v -2.22228093207841 -3.32587844921019 10.8973876056326 +v 2.22228093207841 3.32587844921019 13.9077812881546 +v 3.32587844921019 -2.22228093207841 14.6552707134707 +v -2.82842712474619 -2.82842712474619 10.4868322517275 +v -3.32587844921019 2.22228093207841 10.1498981803165 +v 3.69551813004515 -1.53073372946036 14.9056353149782 +v 3.32587844921019 2.22228093207841 14.6552707134707 +v -3.32587844921019 -2.22228093207841 10.1498981803165 +v -3.69551813004515 1.53073372946036 9.89953357880896 +v -3.69551813004515 -1.53073372946036 9.89953357880896 +v -3.92314112161293 0.780361288064512 9.74535981843486 +v 3.92314112161293 -0.780361288064513 15.0598090753523 +v 3.69551813004515 1.53073372946036 14.9056353149782 +v -4.00000000000001 2.56243672322396E-16 9.69330171034368 +v -3.92314112161293 -0.780361288064515 9.74535981843486 +v 3.92314112161293 0.780361288064516 15.0598090753523 +v 4.00000000000001 2.33615047336546E-16 15.1118671834435 +v 6.96625754306883 6.75 32.5 +v 6.5 6.75 33 +v 5.37912260486461 6.75 16.9707064574746 +v 5.83738467214089 6.75 16.3563658266016 +v 10.5 6.75 -7.63844010796873E-17 +v 9.99999999999999 6.75 3 +v 8.49999999999999 6.75 3 +v 10.5 6.75 33 +v 9.99999999999999 6.75 32.5 +v 5.11700965152127 6.75000000000001 16.1118915531739 +v 4.50557879646464 6.75 15.4543061597503 +v 5.49999999999999 6.75 5 +v 5.49999999999999 6.75 0 +v 8.49999999999999 6.75 5 +v 6.96625754306883 -6.75 32.5 +v 5.83738467214089 -6.75 16.3563658266016 +v 5.37912260486461 -6.75 16.9707064574746 +v 5.11700965152126 -6.75 16.1118915531739 +v 4.50557879646464 -6.75 15.4543061597503 +v 6.5 -6.75 33 +v 10.5 -6.75 33 +v 9.99999999999999 -6.75 32.5 +v 10.5 -6.75 -7.63844010796873E-17 +v 9.99999999999999 -6.75 3 +v 5.49999999999999 -6.75 0 +v 8.49999999999999 -6.75 3 +v 5.49999999999999 -6.75 5 +v 8.49999999999999 -6.75 5 +v 9.99999999999999 -6.75 32.5 +v 9.99999999999999 6.75 32.5 +v 6.96625754306883 6.75 32.5 +v 6.96625754306883 -6.75 32.5 +v 9.99999999999999 -6.75 32.5 +v 9.99999999999999 -2.5 19.9999999999999 +v 9.99999999999999 -2.21364006413302 21.1618079301093 +v 9.99999999999999 6.75 3 +v 9.99999999999999 0.886512217606341 17.6624593932864 +v 9.99999999999999 -0.301341700638305 17.5182278147548 +v 9.99999999999999 -1.42016186682789 22.057459664734 +v 9.99999999999999 -6.75 3 +v 9.99999999999999 -2.21364006413302 18.8381920698905 +v 9.99999999999999 1.87127687042776 18.3421933543979 +v 9.99999999999999 6.75 32.5 +v 9.99999999999999 -0.301341700638307 22.481772185245 +v 9.99999999999999 -1.42016186682789 17.9425403352658 +v 9.99999999999999 2.42735454356513 19.401710839281 +v 9.99999999999999 2.42735454356513 20.5982891607188 +v 9.99999999999999 0.88651221760634 22.3375406067134 +v 9.99999999999999 1.87127687042775 21.6578066456019 +v 5.83738467214089 -6.75 16.3563658266016 +v 6.96625754306883 -6.75 32.5 +v 6.13400884740446 -2.42735454356513 20.5982891607188 +v 6.09217239377503 -2.5 19.9999999999999 +v 6.17341391841827 -2.21364006413302 21.1618079301093 +v 5.83738467214089 6.75 16.3563658266016 +v 6.09217239377503 2.5 19.9999999999999 +v 6.13400884740446 2.42735454356513 20.5982891607188 +v 6.05033594014561 -2.42735454356513 19.401710839281 +v 6.05033594014561 2.42735454356513 19.401710839281 +v 6.0109308691318 -2.21364006413302 18.8381920698905 +v 6.96625754306883 6.75 32.5 +v 6.20809752732073 -1.87127687042775 21.6578066456019 +v 6.0109308691318 2.21364006413303 18.8381920698905 +v 5.97624726022933 -1.87127687042775 18.3421933543979 +v 5.97624726022933 1.87127687042776 18.3421933543979 +v 6.23604398883224 -1.42016186682789 22.057459664734 +v 6.17341391841827 2.21364006413303 21.1618079301093 +v 5.94830079871783 -1.42016186682789 17.9425403352658 +v 6.20809752732073 1.87127687042775 21.6578066456019 +v 5.91735536391627 1.16717187833425E-15 17.4999999999999 +v 5.92021888313259 0.450637594534766 17.540950232632 +v 5.91862997689078 -0.301341700638305 17.5182278147548 +v 6.25562915619099 -0.886512217606339 22.3375406067134 +v 5.94830079871783 1.42016186682789 17.9425403352658 +v 5.92871563135907 0.88651221760634 17.6624593932864 +v 5.92871563135907 -0.886512217606338 17.6624593932864 +v 6.26571481065929 -0.301341700638306 22.481772185245 +v 6.26571481065929 0.30134170063831 22.481772185245 +v 6.23604398883224 1.42016186682789 22.057459664734 +v 6.25562915619099 0.886512217606341 22.3375406067134 +v 9.99999999999999 -1.42016186682789 17.9425403352658 +v 9.99999999999999 -0.301341700638305 17.5182278147548 +v 5.92871563135907 -0.886512217606338 17.6624593932864 +v 5.91862997689078 -0.301341700638305 17.5182278147548 +v 9.99999999999999 -2.21364006413302 18.8381920698905 +v 5.97624726022933 -1.87127687042775 18.3421933543979 +v 5.94830079871783 -1.42016186682789 17.9425403352658 +v 9.99999999999999 -2.5 19.9999999999999 +v 6.05033594014561 -2.42735454356513 19.401710839281 +v 6.0109308691318 -2.21364006413302 18.8381920698905 +v 9.99999999999999 -2.21364006413302 21.1618079301093 +v 6.13400884740446 -2.42735454356513 20.5982891607188 +v 6.09217239377503 -2.5 19.9999999999999 +v 9.99999999999999 -1.42016186682789 22.057459664734 +v 6.20809752732073 -1.87127687042775 21.6578066456019 +v 6.17341391841827 -2.21364006413302 21.1618079301093 +v 9.99999999999999 -0.301341700638307 22.481772185245 +v 6.25562915619099 -0.886512217606339 22.3375406067134 +v 6.23604398883224 -1.42016186682789 22.057459664734 +v 6.26571481065929 -0.301341700638306 22.481772185245 +v 6.26571481065929 0.30134170063831 22.481772185245 +v 9.99999999999999 0.88651221760634 22.3375406067134 +v 6.25562915619099 0.886512217606341 22.3375406067134 +v 6.23604398883224 1.42016186682789 22.057459664734 +v 9.99999999999999 1.87127687042775 21.6578066456019 +v 6.20809752732073 1.87127687042775 21.6578066456019 +v 6.17341391841827 2.21364006413303 21.1618079301093 +v 9.99999999999999 2.42735454356513 20.5982891607188 +v 6.13400884740446 2.42735454356513 20.5982891607188 +v 6.09217239377503 2.5 19.9999999999999 +v 9.99999999999999 2.42735454356513 19.401710839281 +v 6.05033594014561 2.42735454356513 19.401710839281 +v 6.0109308691318 2.21364006413303 18.8381920698905 +v 9.99999999999999 1.87127687042776 18.3421933543979 +v 5.97624726022933 1.87127687042776 18.3421933543979 +v 5.94830079871783 1.42016186682789 17.9425403352658 +v 9.99999999999999 0.886512217606341 17.6624593932864 +v 5.92021888313259 0.450637594534766 17.540950232632 +v 5.92871563135907 0.88651221760634 17.6624593932864 +v 5.91735536391627 1.16717187833425E-15 17.4999999999999 +v -3.5 -3.13397459621557 -1.4328367550024E-15 +v -3 -3.00000000000001 -1.4328367550024E-15 +v -3 -3.00000000000001 4 +v -3.86602540378443 -3.50000000000001 -1.4328367550024E-15 +v -3.43301270189222 -3.25000000000001 4 +v -3.99999999999999 -4 -1.4328367550024E-15 +v -3.86602540378443 -3.50000000000001 4 +v -3.86602540378443 -4.5 -1.4328367550024E-15 +v -3.86602540378443 -4 4 +v -3.5 -4.86602540378443 -1.4328367550024E-15 +v -3.86602540378443 -4.49999999999999 4 +v -3 -4.99999999999999 -1.4328367550024E-15 +v -3.43301270189222 -4.74999999999999 4 +v -2.50000000000001 -4.86602540378443 -1.4328367550024E-15 +v -3 -4.99999999999999 4 +v -2.13397459621557 -4.49999999999999 -1.4328367550024E-15 +v -2.56698729810779 -4.74999999999999 4 +v -2.00000000000001 -4 -1.4328367550024E-15 +v -2.13397459621557 -4.49999999999999 4 +v -2.13397459621557 -3.50000000000001 -1.4328367550024E-15 +v -2.13397459621557 -4 4 +v -2.50000000000001 -3.13397459621557 -1.4328367550024E-15 +v -2.13397459621558 -3.50000000000001 4 +v -2.56698729810779 -3.25000000000001 4 +v -3 -4 4.60086061902756 +v -3 -3.00000000000001 4 +v -2.56698729810779 -3.25000000000001 4 +v -3 -4 4.60086061902756 +v -2.13397459621558 -3.50000000000001 4 +v -2.13397459621557 -4 4 +v -3 -4 4.60086061902756 +v -2.13397459621557 -4.49999999999999 4 +v -2.56698729810779 -4.74999999999999 4 +v -3 -4 4.60086061902756 +v -3 -4.99999999999999 4 +v -3.43301270189222 -4.74999999999999 4 +v -3 -4 4.60086061902756 +v -3.86602540378443 -4.49999999999999 4 +v -3.86602540378443 -4 4 +v -3 -4 4.60086061902756 +v -3.86602540378443 -3.50000000000001 4 +v -3.43301270189222 -3.25000000000001 4 +v -3 -4 4.60086061902756 +v -3 -4 4.60086061902756 +v -3 -4 4.60086061902756 +v -3 -4 4.60086061902756 +v -3 -4 4.60086061902756 +v -3 -4 4.60086061902756 +# 777 vertices + +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 4.42818860291655E-17 -0.370138155339915 0.928976719816791 +vn 1.93549674076193E-17 -0.161781996552765 0.986826522541526 +vn 1.93549674076193E-17 -0.161781996552765 0.986826522541526 +vn 6.71382328757693E-17 -0.561187065362382 0.827688998156891 +vn 4.42818860291656E-17 -0.370138155339915 0.928976719816791 +vn 8.68552705718886E-17 -0.725995491923131 0.687699458853424 +vn 6.71382328757693E-17 -0.561187065362382 0.827688998156891 +vn 1.02511052348766E-16 -0.856857176167589 0.515553857177022 +vn 8.68552705718887E-17 -0.725995491923131 0.687699458853424 +vn 1.13373531250667E-16 -0.947653171182803 0.319301530135979 +vn 1.02511052348766E-16 -0.856857176167589 0.515553857177022 +vn 1.18934789836917E-16 -0.99413795715436 0.108119018423941 +vn 1.13373531250667E-16 -0.947653171182803 0.319301530135979 +vn 1.19636101791504E-16 -1 1.22464679914735E-16 +vn 1.18934789836917E-16 -0.99413795715436 0.108119018423941 +vn 1.18934789836917E-16 -0.994137957154359 -0.108119018423943 +vn 1.18934789836917E-16 -0.994137957154359 -0.108119018423943 +vn 1.13373531250667E-16 -0.947653171182802 -0.31930153013598 +vn 1.02511052348766E-16 -0.856857176167589 -0.515553857177022 +vn 1.13373531250667E-16 -0.947653171182802 -0.31930153013598 +vn 8.68552705718887E-17 -0.725995491923131 -0.687699458853423 +vn 1.02511052348766E-16 -0.856857176167589 -0.515553857177022 +vn 6.71382328757695E-17 -0.561187065362383 -0.82768899815689 +vn 8.68552705718887E-17 -0.725995491923131 -0.687699458853423 +vn 4.42818860291655E-17 -0.370138155339914 -0.928976719816791 +vn 6.71382328757695E-17 -0.561187065362383 -0.82768899815689 +vn 1.93549674076193E-17 -0.161781996552765 -0.986826522541526 +vn 4.42818860291655E-17 -0.370138155339914 -0.928976719816791 +vn -6.47696797840595E-18 5.41389085854176E-02 -0.998533413851124 +vn 1.93549674076193E-17 -0.161781996552765 -0.986826522541526 +vn -6.47696797840595E-18 5.41389085854176E-02 -0.998533413851124 +vn -3.20060475403938E-17 0.267528338529221 -0.963549992519223 +vn -3.20060475403938E-17 0.267528338529221 -0.963549992519223 +vn -5.60385598915598E-17 0.468408440699791 -0.883512044446023 +vn -5.60385598915598E-17 0.468408440699791 -0.883512044446023 +vn -7.74507714645824E-17 0.647386284781828 -0.762162055127636 +vn -7.74507714645824E-17 0.647386284781828 -0.762162055127636 +vn -9.52414710442709E-17 0.796093065705644 -0.605174215193765 +vn -9.52414710442709E-17 0.796093065705644 -0.605174215193765 +vn -1.08578785291221E-16 0.907575419670957 -0.419889101560266 +vn -1.08578785291221E-16 0.907575419670957 -0.419889101560266 +vn -1.16839076214607E-16 0.976620555710087 -0.214970440211024 +vn -1.16839076214607E-16 0.976620555710087 -0.214970440211024 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.16839076214607E-16 0.976620555710087 0.214970440211024 +vn -1.16839076214607E-16 0.976620555710087 0.214970440211024 +vn -1.08578785291222E-16 0.907575419670957 0.419889101560264 +vn -1.08578785291222E-16 0.907575419670957 0.419889101560264 +vn -9.52414710442708E-17 0.796093065705643 0.605174215193766 +vn -9.52414710442708E-17 0.796093065705643 0.605174215193766 +vn -7.74507714645822E-17 0.647386284781827 0.762162055127637 +vn -7.74507714645822E-17 0.647386284781827 0.762162055127637 +vn -5.60385598915598E-17 0.468408440699791 0.883512044446023 +vn -5.60385598915598E-17 0.468408440699791 0.883512044446023 +vn -3.20060475403939E-17 0.267528338529222 0.963549992519223 +vn -3.20060475403939E-17 0.267528338529222 0.963549992519223 +vn -6.41986408231434E-18 5.36615953393614E-02 0.998559178609678 +vn -6.47696797840592E-18 5.41389085854174E-02 0.998533413851124 +vn -7.32559845607161E-33 6.12323399573677E-17 1 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn -1.19636101791504E-16 1 0 +vn -1 1.25293258037967E-16 -8.44169346989549E-17 +vn -0.382683432365087 0.923879532511288 0 +vn -0.831469612302544 0.555570233019604 0 +vn 1.84041775748085E-15 1 1.65461738644629E-31 +vn 1.84041775748085E-15 1 1.65461738644629E-31 +vn -1 -1.19636101791504E-16 -8.44169346989549E-17 +vn -1 -1.19636101791504E-16 -8.44169346989549E-17 +vn -1 -1.19636101791504E-16 -8.44169346989549E-17 +vn -1 -1.19636101791504E-16 -8.44169346989549E-17 +vn -1 -1.19636101791504E-16 -8.44169346989549E-17 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.560792146059838 6.70909862698907E-17 -0.827956622606282 +vn 0.997564050259825 1.19344674260429E-16 -0.069756473744118 +vn 0.997564050259825 1.19344674260429E-16 -0.069756473744118 +vn 0.997564050259825 1.19344674260429E-16 -0.069756473744118 +vn 0.997564050259825 1.19344674260429E-16 -0.069756473744118 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn -1 -1.19636101791504E-16 0 +vn -1 -1.19636101791504E-16 0 +vn -1 -1.19636101791504E-16 0 +vn -1 -1.19636101791504E-16 0 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn 1 1.19636101791504E-16 -8.67361737988404E-16 +vn 1 1.19636101791504E-16 -8.67361737988404E-16 +vn 1 1.19636101791504E-16 -8.67361737988404E-16 +vn 1 1.19636101791504E-16 -8.67361737988404E-16 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -5.78241158658935E-16 -6.91785181173575E-32 1 +vn -1 -1.19636101791504E-16 0 +vn -1 -1.19636101791504E-16 0 +vn -1 -1.19636101791504E-16 0 +vn -1 -1.19636101791504E-16 0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1.19636101791504E-16 1 -0 +vn -1 -1.19636101791504E-16 0 +vn -0.555570233019603 -0.831469612302545 0 +vn 1.069046861449E-15 -1 8.01463461004917E-32 +vn -1 -2.42100781706239E-16 -8.44169346989549E-17 +vn -0.923879532511287 -0.382683432365089 0 +vn 1.069046861449E-15 -1 8.01463461004917E-32 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn -1 -1.19636101791504E-16 1.22464679914735E-16 +vn -1 -1.19636101791504E-16 1.22464679914735E-16 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -7.32559845607161E-33 6.12323399573677E-17 1 +vn -7.32559845607161E-33 6.12323399573677E-17 1 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn 1 1.80868441748872E-16 0 +vn 1 1.80868441748872E-16 0 +vn 1.0842021724855E-16 1.29709721470046E-32 -1 +vn 1.0842021724855E-16 1.29709721470046E-32 -1 +vn 1.0842021724855E-16 1.29709721470046E-32 -1 +vn 1.0842021724855E-16 1.29709721470046E-32 -1 +vn 1.0842021724855E-16 1.29709721470046E-32 -1 +vn 1.0842021724855E-16 1.29709721470046E-32 -1 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn 1 1.19636101791504E-16 0 +vn -1.19636101791504E-16 1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1 5.84037618341363E-17 0 +vn 1 5.84037618341363E-17 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 7.32559845607161E-33 -6.12323399573677E-17 1 +vn 7.32559845607161E-33 -6.12323399573677E-17 1 +vn 1 5.84037618341363E-17 0 +vn 1 5.84037618341363E-17 0 +vn 7.32559845607161E-33 -6.12323399573677E-17 1 +vn 7.32559845607161E-33 -6.12323399573677E-17 1 +vn 1.19636101791504E-16 -1 0 +vn 1 1.19636101791504E-16 0 +vn 6.12323399573677E-17 7.32559845607161E-33 1 +vn -0.257519037455028 -0.446036056788342 -0.85716730070211 +vn 6.16171475564398E-17 -0.515038074910057 -0.857167300702111 +vn -0.257519037455028 -0.446036056788342 -0.85716730070211 +vn -0.515038074910057 -9.31541340503206E-17 -0.857167300702111 +vn -0.446036056788342 -0.257519037455029 -0.857167300702111 +vn -0.515038074910057 -9.31541340503206E-17 -0.857167300702111 +vn -0.257519037455029 0.446036056788342 -0.85716730070211 +vn -0.446036056788342 0.257519037455028 -0.857167300702111 +vn -0.257519037455029 0.446036056788342 -0.85716730070211 +vn 0.257519037455028 0.446036056788342 -0.857167300702111 +vn -2.61749642819756E-16 0.515038074910057 -0.857167300702111 +vn 0.257519037455028 0.446036056788342 -0.857167300702111 +vn 0.515038074910057 6.13673810497092E-16 -0.857167300702111 +vn 0.446036056788342 0.257519037455029 -0.857167300702111 +vn 0.515038074910057 6.13673810497092E-16 -0.857167300702111 +vn 0.25751903745503 -0.446036056788342 -0.857167300702111 +vn 0.446036056788342 -0.257519037455028 -0.857167300702111 +vn 0.25751903745503 -0.446036056788342 -0.857167300702111 +vn 6.16171475564398E-17 -0.515038074910057 -0.857167300702111 +vn 0.446036056788342 -0.257519037455028 -0.857167300702111 +vn 0.446036056788342 0.257519037455029 -0.85716730070211 +vn -1.24691120544201E-16 0.515038074910057 -0.857167300702111 +vn -0.446036056788342 0.257519037455028 -0.857167300702111 +vn -0.446036056788342 -0.257519037455029 -0.857167300702111 +vn 0.500000000000001 -0.866025403784438 0 +vn 3.64565461620975E-16 -1 0 +vn 3.64565461620975E-16 -1 0 +vn 0.866025403784439 -0.499999999999999 0 +vn 0.500000000000002 -0.866025403784438 0 +vn 1 1.19151154136373E-15 0 +vn 0.866025403784439 -0.499999999999999 0 +vn 0.866025403784438 0.500000000000001 0 +vn 1 9.86997839779918E-16 0 +vn 0.499999999999999 0.866025403784439 0 +vn 0.866025403784438 0.500000000000001 0 +vn -6.86189991556302E-16 1 0 +vn 0.499999999999999 0.866025403784439 0 +vn -0.5 0.866025403784438 0 +vn -6.86189991556302E-16 1 0 +vn -0.866025403784439 0.5 0 +vn -0.5 0.866025403784438 -0 +vn -1 -1.80868441748872E-16 0 +vn -0.866025403784439 0.5 0 +vn -0.866025403784438 -0.5 0 +vn -1 -1.19636101791504E-16 -0 +vn -0.5 -0.866025403784439 0 +vn -0.866025403784438 -0.5 0 +vn -0.5 -0.866025403784439 0 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 5.10212787052002E-17 6.10398689270802E-33 -1 +vn 1 -2.46519032881566E-32 0 +vn 1 -2.46519032881566E-32 0 +vn 1 -2.46519032881566E-32 0 +vn 1 -2.46519032881566E-32 0 +vn 0.707106781186541 -1.23259516440783E-32 -0.707106781186554 +vn 0.707106781186541 -1.23259516440783E-32 -0.707106781186554 +vn 0.707106781186541 -1.23259516440783E-32 -0.707106781186554 +vn 0.707106781186541 -1.23259516440783E-32 -0.707106781186554 +vn 1 -2.46519032881566E-32 0 +vn 1 -2.46519032881566E-32 0 +vn 1 -2.46519032881566E-32 0 +vn 1 -2.46519032881566E-32 0 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -1 -1.19636101791504E-16 8.67361737988404E-16 +vn -1 -1.19636101791504E-16 8.67361737988404E-16 +vn -1 -1.19636101791504E-16 8.67361737988404E-16 +vn -1 -1.19636101791504E-16 8.67361737988404E-16 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn -2.89120579329468E-16 -3.45892590586788E-32 -1 +vn 0.70710678118654 -0.707106781186555 0 +vn 0.70710678118654 -0.707106781186555 0 +vn 0.70710678118654 -0.707106781186555 0 +vn 0.70710678118654 -0.707106781186555 0 +vn 8.45954988514974E-17 -0.707106781186555 0.70710678118654 +vn 8.45954988514974E-17 -0.707106781186555 0.70710678118654 +vn 8.45954988514974E-17 -0.707106781186555 0.70710678118654 +vn 8.45954988514974E-17 -0.707106781186555 0.70710678118654 +vn -0.705384304606643 -0.707106781186544 4.93252756161275E-02 +vn -0.705384304606643 -0.707106781186544 4.93252756161275E-02 +vn -0.705384304606643 -0.707106781186544 4.93252756161275E-02 +vn -0.705384304606643 -0.707106781186544 4.93252756161275E-02 +vn -0.396539929315069 -0.707106781186545 0.585453742373215 +vn -0.612713381233684 -0.707106781186545 0.352962196923677 +vn -0.612713381233684 -0.707106781186545 0.352962196923677 +vn -0.705384304606642 -0.707106781186545 4.93252756161269E-02 +vn -0.705384304606642 -0.707106781186545 4.93252756161269E-02 +vn -0.396539929315069 -0.707106781186545 0.585453742373215 +vn 0.70710678118655 0.707106781186545 0 +vn 0.70710678118655 0.707106781186545 0 +vn 0.70710678118655 0.707106781186545 0 +vn 0.70710678118655 0.707106781186545 0 +vn -8.45954988514962E-17 0.707106781186545 0.70710678118655 +vn -8.45954988514962E-17 0.707106781186545 0.70710678118655 +vn -8.45954988514962E-17 0.707106781186545 0.70710678118655 +vn -8.45954988514962E-17 0.707106781186545 0.70710678118655 +vn -0.705384304606639 0.707106781186548 4.93252756161273E-02 +vn -0.705384304606639 0.707106781186548 4.93252756161273E-02 +vn -0.705384304606639 0.707106781186548 4.93252756161273E-02 +vn -0.705384304606639 0.707106781186548 4.93252756161273E-02 +vn -0.705384304606639 0.707106781186548 4.93252756161273E-02 +vn -0.612713381233682 0.707106781186548 0.352962196923676 +vn -0.612713381233682 0.707106781186548 0.352962196923676 +vn -0.396539929315068 0.707106781186548 0.585453742373213 +vn -0.396539929315068 0.707106781186548 0.585453742373213 +vn -0.705384304606639 0.707106781186548 4.93252756161267E-02 +vn 0.500000000000001 0.866025403784438 0 +vn 1.25293258037967E-16 1 0 +vn 1.25293258037967E-16 1 0 +vn 0.866025403784439 0.499999999999999 0 +vn 0.500000000000001 0.866025403784438 0 +vn 1 -9.52239337780724E-16 0 +vn 0.866025403784439 0.499999999999999 0 +vn 0.866025403784438 -0.500000000000001 0 +vn 1 -9.52239337780724E-16 0 +vn 0.499999999999999 -0.866025403784439 0 +vn 0.866025403784438 -0.500000000000001 0 +vn -4.46917787973294E-16 -1 0 +vn 0.499999999999999 -0.866025403784439 0 +vn -0.5 -0.866025403784439 0 +vn -4.46917787973294E-16 -1 0 +vn -0.866025403784439 -0.5 0 +vn -0.5 -0.866025403784439 0 +vn -1 1.63640843090895E-16 -0 +vn -0.866025403784439 -0.5 0 +vn -0.866025403784439 0.5 -0 +vn -1 1.63640843090895E-16 -0 +vn -0.5 0.866025403784439 -0 +vn -0.866025403784439 0.5 -0 +vn -0.5 0.866025403784439 -0 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -0 0 1 +vn -1 -2.80448366759168E-16 0 +vn -0.98078528040323 -0.195090322016129 0 +vn -0.923879532511287 -0.38268343236509 0 +vn -1 -5.84037618341363E-17 -0 +vn -0.923879532511287 -0.38268343236509 0 +vn -0.831469612302545 -0.555570233019602 0 +vn -0.707106781186547 -0.707106781186548 0 +vn -0.707106781186547 -0.707106781186548 0 +vn -0.555570233019602 -0.831469612302546 0 +vn -0.38268343236509 -0.923879532511287 0 +vn -0.38268343236509 -0.923879532511287 0 +vn -0.195090322016128 -0.98078528040323 0 +vn -9.72043327055966E-17 -1 0 +vn -9.72043327055966E-17 -1 0 +vn 0.195090322016128 -0.98078528040323 0 +vn 0.38268343236509 -0.923879532511287 0 +vn 0.38268343236509 -0.923879532511287 0 +vn 0.555570233019602 -0.831469612302545 0 +vn 0.707106781186547 -0.707106781186547 0 +vn 0.707106781186547 -0.707106781186547 0 +vn 0.831469612302545 -0.555570233019602 0 +vn 0.923879532511287 -0.382683432365089 0 +vn 1 -6.4060918080599E-17 0 +vn 0.923879532511287 -0.382683432365089 0 +vn 0.98078528040323 -0.195090322016128 0 +vn 1 -6.4060918080599E-17 0 +vn 0.98078528040323 0.195090322016128 0 +vn 0.923879532511287 0.38268343236509 0 +vn 0.707106781186548 0.707106781186547 0 +vn 0.923879532511287 0.38268343236509 0 +vn 0.831469612302545 0.555570233019602 0 +vn 0.382683432365089 0.923879532511287 0 +vn 0.707106781186548 0.707106781186547 0 +vn 0.555570233019602 0.831469612302545 0 +vn 1.25293258037967E-16 1 0 +vn 0.382683432365089 0.923879532511287 0 +vn 0.195090322016129 0.98078528040323 0 +vn -0.38268343236509 0.923879532511287 -0 +vn -1.19636101791504E-16 1 -0 +vn -0.195090322016128 0.98078528040323 -0 +vn -0.707106781186548 0.707106781186547 -0 +vn -0.38268343236509 0.923879532511287 -0 +vn -0.555570233019602 0.831469612302545 -0 +vn -0.923879532511287 0.38268343236509 -0 +vn -0.707106781186547 0.707106781186548 -0 +vn -0.831469612302545 0.555570233019602 -0 +vn -0.923879532511287 0.382683432365089 -0 +vn -0.98078528040323 0.195090322016128 -0 +vn -0.39653992931507 -0.707106781186545 0.585453742373215 +vn -0.39653992931507 -0.707106781186545 0.585453742373215 +vn -0.39653992931507 -0.707106781186545 0.585453742373215 +vn -0.39653992931507 -0.707106781186545 0.585453742373215 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn -0.70710678118655 -0.707106781186545 4.32978028117749E-17 +vn -0.70710678118655 -0.707106781186545 4.32978028117749E-17 +vn -0.70710678118655 -0.707106781186545 4.32978028117749E-17 +vn -0.70710678118655 -0.707106781186545 4.32978028117749E-17 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -0.707106781186548 0.707106781186547 4.32978028117747E-17 +vn -0.707106781186548 0.707106781186547 4.32978028117747E-17 +vn -0.707106781186548 0.707106781186547 4.32978028117747E-17 +vn -0.707106781186548 0.707106781186547 4.32978028117747E-17 +vn -0.396539929315068 0.707106781186549 0.585453742373212 +vn -0.396539929315068 0.707106781186549 0.585453742373212 +vn -0.396539929315068 0.707106781186549 0.585453742373212 +vn -0.396539929315068 0.707106781186549 0.585453742373212 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn -1 -1.19636101791504E-16 6.12323399573677E-17 +vn -0.883400290372331 -1.05686567061628E-16 0.468619170510641 +vn -0.883400290372331 -1.05686567061628E-16 0.468619170510641 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.70710678118655 -0.707106781186545 0 +vn -0.70710678118655 -0.707106781186545 0 +vn -0.624658335824442 -0.707106781186545 0.33136379326209 +vn -0.624658335824442 -0.707106781186545 0.33136379326209 +vn -0.396539929315069 -0.707106781186545 0.585453742373215 +vn -0.396539929315069 -0.707106781186545 0.585453742373215 +vn -0.396539929315068 0.707106781186548 0.585453742373213 +vn -0.396539929315068 0.707106781186548 0.585453742373213 +vn -0.62465833582444 0.707106781186548 0.331363793262089 +vn -0.62465833582444 0.707106781186548 0.331363793262089 +vn -0.707106781186547 0.707106781186548 -1.11022302462516E-16 +vn -0.707106781186547 0.707106781186548 -1.11022302462516E-16 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn -0.560792146059838 -6.70909862698907E-17 0.827956622606282 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn 1.19636101791504E-16 -1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn -1.19636101791504E-16 1 0 +vn 0 0 1 +vn 0 0 1 +vn 0 0 1 +vn 0 0 1 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn 1 1.19636101791504E-16 0 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -0.997564050259825 -1.19344674260429E-16 0.069756473744118 +vn -6.79610518640934E-17 0.568064746731156 0.822983865893657 +vn -1.44205385486357E-17 0.120536680255322 0.992708874098054 +vn -4.24235463619856E-17 0.354604887042536 0.935016242685415 +vn -1.44205385486357E-17 0.120536680255322 0.992708874098054 +vn -1.05932507216948E-16 0.88545602565321 0.464723172043768 +vn -8.95489080602327E-17 0.748510748171102 0.663122658240795 +vn -6.79610518640935E-17 0.568064746731156 0.822983865893656 +vn -1.19636101791504E-16 1 2.44929359829471E-16 +vn -1.16159694103211E-16 0.970941817426052 0.239315664287558 +vn -1.05932507216948E-16 0.88545602565321 0.464723172043768 +vn -1.05932507216948E-16 0.88545602565321 -0.464723172043768 +vn -1.16159694103211E-16 0.970941817426052 -0.239315664287558 +vn -1.19636101791504E-16 1 -0 +vn -6.79610518640935E-17 0.568064746731156 -0.822983865893656 +vn -8.95489080602327E-17 0.748510748171102 -0.663122658240795 +vn -1.05932507216948E-16 0.88545602565321 -0.464723172043768 +vn -1.44205385486358E-17 0.120536680255323 -0.992708874098054 +vn -4.24235463619856E-17 0.354604887042536 -0.935016242685415 +vn -6.79610518640935E-17 0.568064746731156 -0.822983865893656 +vn -1.44205385486358E-17 0.120536680255323 -0.992708874098054 +vn 1.44205385486358E-17 -0.120536680255324 -0.992708874098054 +vn 4.24235463619856E-17 -0.354604887042536 -0.935016242685415 +vn 4.24235463619856E-17 -0.354604887042536 -0.935016242685415 +vn 6.79610518640935E-17 -0.568064746731156 -0.822983865893656 +vn 8.95489080602326E-17 -0.748510748171101 -0.663122658240796 +vn 8.95489080602326E-17 -0.748510748171101 -0.663122658240796 +vn 1.05932507216948E-16 -0.88545602565321 -0.464723172043768 +vn 1.16159694103211E-16 -0.970941817426052 -0.239315664287558 +vn 1.16159694103211E-16 -0.970941817426052 -0.239315664287558 +vn 1.19636101791504E-16 -1 0 +vn 1.16159694103211E-16 -0.970941817426052 0.239315664287558 +vn 1.16159694103211E-16 -0.970941817426052 0.239315664287558 +vn 1.05932507216948E-16 -0.88545602565321 0.464723172043768 +vn 8.95489080602327E-17 -0.748510748171102 0.663122658240795 +vn 8.95489080602327E-17 -0.748510748171102 0.663122658240795 +vn 6.79610518640935E-17 -0.568064746731156 0.822983865893656 +vn 4.24235463619856E-17 -0.354604887042536 0.935016242685415 +vn 2.15650100523359E-17 -0.180255037813906 0.983619906947144 +vn 4.24235463619856E-17 -0.354604887042536 0.935016242685415 +vn 2.19767953682148E-32 -1.83697019872103E-16 1 +vn 0.500000000000001 -0.866025403784438 0 +vn 3.64565461620975E-16 -1 0 +vn 3.64565461620975E-16 -1 0 +vn 0.866025403784439 -0.499999999999999 0 +vn 0.500000000000002 -0.866025403784437 0 +vn 1 1.19151154136373E-15 0 +vn 0.866025403784439 -0.499999999999999 0 +vn 0.866025403784438 0.500000000000001 0 +vn 1 9.86997839779918E-16 0 +vn 0.499999999999999 0.866025403784439 0 +vn 0.866025403784438 0.500000000000001 0 +vn -6.86189991556302E-16 1 0 +vn 0.499999999999999 0.866025403784439 0 +vn -0.5 0.866025403784438 0 +vn -6.86189991556302E-16 1 0 +vn -0.866025403784439 0.5 0 +vn -0.5 0.866025403784438 -0 +vn -1 -1.80868441748872E-16 0 +vn -0.866025403784439 0.5 0 +vn -0.866025403784438 -0.5 0 +vn -1 -1.19636101791504E-16 -0 +vn -0.5 -0.866025403784439 0 +vn -0.866025403784438 -0.5 0 +vn -0.5 -0.866025403784439 0 +vn -0.257519037455028 -0.446036056788342 -0.85716730070211 +vn 6.16171475564398E-17 -0.515038074910057 -0.857167300702111 +vn -0.257519037455028 -0.446036056788342 -0.85716730070211 +vn -0.515038074910057 -9.31541340503206E-17 -0.857167300702111 +vn -0.446036056788342 -0.257519037455029 -0.857167300702111 +vn -0.515038074910057 -9.31541340503206E-17 -0.857167300702111 +vn -0.257519037455029 0.446036056788342 -0.857167300702111 +vn -0.446036056788342 0.257519037455028 -0.857167300702111 +vn -0.257519037455029 0.446036056788342 -0.857167300702111 +vn 0.257519037455028 0.446036056788342 -0.857167300702111 +vn -2.61749642819756E-16 0.515038074910057 -0.857167300702111 +vn 0.257519037455028 0.446036056788342 -0.857167300702111 +vn 0.515038074910057 6.13673810497092E-16 -0.857167300702111 +vn 0.446036056788342 0.257519037455029 -0.857167300702111 +vn 0.515038074910057 6.13673810497092E-16 -0.857167300702111 +vn 0.25751903745503 -0.446036056788342 -0.857167300702111 +vn 0.446036056788342 -0.257519037455028 -0.857167300702111 +vn 0.25751903745503 -0.446036056788342 -0.857167300702111 +vn 6.16171475564398E-17 -0.515038074910057 -0.857167300702111 +vn 0.446036056788342 -0.257519037455028 -0.857167300702111 +vn 0.446036056788342 0.257519037455029 -0.85716730070211 +vn -1.24691120544201E-16 0.515038074910057 -0.857167300702111 +vn -0.446036056788342 0.257519037455028 -0.857167300702111 +vn -0.446036056788342 -0.257519037455029 -0.857167300702111 +# 777 normals + +vt 0 0 +vt 2.5 0 +vt 2.4853448928859 -0.270297546059856 +vt 2.36913292795701 -0.798253825339951 +vt 2.14214294041897 -1.28888464294255 +vt 1.81498872980783 -1.71924864713356 +vt 1.40296766340596 -2.06922249539222 +vt 0.925345388349786 -2.32244179954198 +vt 0.404454991381912 -2.46706630635381 +vt -0.135347271463544 -2.49633353462781 +vt -0.668820846323052 -2.40887498129806 +vt -1.17102110174948 -2.20878011111505 +vt -1.61846571195457 -1.90540513781909 +vt -1.99023266426411 -1.51293553798441 +vt -2.26893854917739 -1.04972275390066 +vt -2.44155138927522 -0.537426100527559 +vt -2.5 0 +vt -2.44155138927522 0.537426100527559 +vt -2.26893854917739 1.04972275390066 +vt -1.99023266426411 1.51293553798441 +vt -1.61846571195457 1.90540513781909 +vt -1.17102110174947 2.20878011111505 +vt -0.668820846323052 2.40887498129806 +vt -0.135347271463543 2.49633353462781 +vt 0.404454991381912 2.46706630635381 +vt 0.925345388349786 2.32244179954198 +vt 1.40296766340596 2.06922249539223 +vt 1.81498872980783 1.71924864713356 +vt 2.14214294041897 1.28888464294255 +vt 2.36913292795701 0.798253825339948 +vt 2.4853448928859 0.270297546059853 +vt 1949.95406084884 10.9875288315512 +vt 1733.2924985323 10.9875288315512 +vt 1733.2924985323 7.25221530698284 +vt 2166.61562316537 10.9875288315512 +vt 1949.95406084884 7.24210217629259 +vt 2383.27718548191 10.9875288315512 +vt 2166.61562316537 7.22439535763084 +vt 2599.93874779845 10.9875288315512 +vt 2383.27718548191 7.19992280215851 +vt 2816.60031011499 10.9875288315512 +vt 2599.93874779845 7.1698288193702 +vt 3033.26187243153 10.9875288315512 +vt 2816.60031011499 7.13552057045401 +vt -3141.59265358979 10.9875288315512 +vt 3033.26187243153 7.0986022709984 +vt -3033.26187243152 7.06080017965417 +vt -3033.26187243152 10.9875288315512 +vt -2816.60031011499 10.9875288315512 +vt -2599.93874779845 10.9875288315512 +vt -2816.60031011499 7.02388188019856 +vt -2383.27718548191 10.9875288315512 +vt -2599.93874779845 6.98957363128237 +vt -2166.61562316538 10.9875288315512 +vt -2383.27718548191 6.95947964849405 +vt -1949.95406084884 10.9875288315512 +vt -2166.61562316538 6.93500709302173 +vt -1733.2924985323 10.9875288315512 +vt -1949.95406084884 6.91730027435998 +vt -1516.63093621576 10.9875288315512 +vt -1733.2924985323 6.90718714366973 +vt -1516.63093621576 6.9051405797021 +vt -1299.96937389922 6.91125627751364 +vt -1299.96937389922 10.9875288315512 +vt -1083.30781158269 6.92524827387179 +vt -1083.30781158269 10.9875288315512 +vt -866.646249266149 6.94646231857782 +vt -866.646249266149 10.9875288315512 +vt -649.984686949612 6.97390646647901 +vt -649.984686949612 10.9875288315512 +vt -433.323124633076 7.00629745972146 +vt -433.323124633075 10.9875288315512 +vt -216.661562316537 7.04212073146116 +vt -216.661562316538 10.9875288315512 +vt 0 7.07970122532628 +vt 0 10.9875288315512 +vt 216.661562316537 7.11728171919141 +vt 216.661562316537 10.9875288315512 +vt 433.323124633074 7.15310499093111 +vt 433.323124633074 10.9875288315512 +vt 649.984686949614 7.18549598417356 +vt 649.984686949613 10.9875288315512 +vt 866.64624926615 7.21294013207474 +vt 866.64624926615 10.9875288315512 +vt 1083.30781158269 7.23415417678078 +vt 1083.30781158269 10.9875288315512 +vt 1299.96937389922 7.24814617313892 +vt 1299.96937389923 10.9875288315512 +vt 1517.10894433493 7.25324758421617 +vt 1516.63093621576 10.9875288315512 +vt 1570.7963267949 7.25451825518504 +vt 1.17944092586136 7.15445499138191 +vt 1.43835782966466 5.57897889825052 +vt 1.23777434820211 6.08117915367695 +vt 6.14042048764178 6.61662895633675 +vt 6.06728876518745 6.08117915367695 +vt 6.1256221875282 7.15445499138191 +vt 5.37597843380083 8.56498872980783 +vt 5.72680688252802 8.15296766340596 +vt 1.32441859097887 7.67534538834979 +vt 1.7424736137258 5.13153428804543 +vt 5.8667052837249 5.57897889825052 +vt 4.94456352457379 8.89214294041897 +vt 1.57825623086154 8.15296766340596 +vt 2.13590158432323 4.75976733573589 +vt 5.56258949966377 5.13153428804543 +vt 4.45273463651911 9.11913292795701 +vt 1.92908467958873 8.56498872980783 +vt 2.6002454868156 4.48106145082261 +vt 5.16916152906633 4.75976733573589 +vt 3.9234891418137 9.2353448928859 +vt 2.36049958881577 8.89214294041897 +vt 3.11379311640357 4.30844861072478 +vt 4.70481762657395 4.48106145082261 +vt 3.38157397157586 9.2353448928859 +vt 2.85232847687044 9.11913292795701 +vt 3.65253155669478 4.25 +vt 4.19126999698599 4.30844861072478 +vt 6.15863630189771 6.75 +vt 5.98064452241069 7.67534538834979 +vt 1.1501022299988 6.61465272853646 +vt 3141.59265358979 0 +vt 1570.7963267949 0 +vt 1570.7963267949 -1570.7963267949 +vt 6283.18530717959 -0.5 +vt 5105.08806208341 -20.3404118345279 +vt 5694.1366846315 -20.4923979157412 +vt 4712.38898038469 -0.5 +vt 4712.38898038469 -20.2108121324191 +vt 6283.18530717959 -20.5494724744878 +vt 0.5 13 +vt 0.5 0.5 +vt 20.5494724744878 0.5 +vt 20.5494724744878 13 +vt -1.61528350112233 0.500000000000001 +vt -1.51350861153515 0.222214883490199 +vt -1.2424882659736 3.80602337443567E-02 +vt 0 13.5 +vt -1.61528350112233 13 +vt -1.0113871298051 13.5 +vt -1.3468939775375 13.4157348061513 +vt -1.0113871298051 0 +vt -1.56931462702292 13.1913417161825 +vt 0 0 +vt 0 13.5 +vt 0 0 +vt 16.1830552827095 0 +vt 16.1830552827095 13.5 +vt -13.5 3.03374245693117 +vt -13.5 0 +vt 0 0 +vt 0 3.03374245693117 +vt 0 0 +vt 0 -29.5 +vt 13.5 -29.5 +vt 13.5 0 +vt 0 13.5 +vt 0 0 +vt 1.5 0 +vt 1.5 13.5 +vt 0 13.5 +vt 0 0 +vt 2 0 +vt 2 13.5 +vt 0 13.5 +vt 0 0 +vt 3 0 +vt 3 13.5 +vt 0 0 +vt 0 -5 +vt 13.5 -5 +vt 13.5 0 +vt 11.5 15.7891878675809 +vt 11.5 35.5 +vt 13.4662575430688 32.5 +vt 19.5 35.5 +vt 16.5 32.5 +vt 19.5 0 +vt 16.5 3 +vt 12 0 +vt 15 3 +vt 12 5 +vt 15 5 +vt 12.3373846721409 16.3563658266016 +vt 3141.59265358979 -35.5 +vt 2159.84494934298 -15.6010382624233 +vt 1570.7963267949 -15.7891878675809 +vt 3141.59265358979 -15.4505275255122 +vt 2748.89357189107 -15.4763065090704 +vt 1570.7963267949 -35.5 +vt 1570.7963267949 13 +vt 1570.7963267949 0.5 +vt 3141.59265358979 0.5 +vt 3141.59265358979 13 +vt 0 -0.5 +vt 0 -8.5 +vt 1570.7963267949 -8.5 +vt 1570.7963267949 -0.5 +vt 0 5.42101086242752E-17 +vt 0 -35.5 +vt 1570.7963267949 -35.5 +vt 1570.7963267949 5.42101086242752E-17 +vt 7.5 13.5 +vt 7.5 0 +vt 8 0.5 +vt 0 0 +vt 8 13 +vt 0 13.5 +vt 19.5 0 +vt 16.5 3 +vt 15 3 +vt 19.5 35.5 +vt 16.5 32.5 +vt 11.5 35.5 +vt 13.4662575430688 32.5 +vt 11.5 15.7891878675809 +vt 12.3373846721409 16.3563658266016 +vt 12 5 +vt 12 0 +vt 15 5 +vt 3141.59265358979 1570.7963267949 +vt 1570.7963267949 0 +vt 3141.59265358979 0 +vt 0.5 -0.5 +vt 0.5 -8.5 +vt 13 -8.5 +vt 13 -0.5 +vt -13 35.5 +vt -13 0 +vt -0.5 0 +vt -0.5 35.5 +vt 1570.7963267949 0 +vt 0 0 +vt 0 -1570.7963267949 +vt 0 35.5 +vt 0 8.67361737988404E-16 +vt 1570.7963267949 8.67361737988404E-16 +vt 1570.7963267949 35.5 +vt 0 -11.5 +vt 0 -19.5 +vt 1570.7963267949 -19.5 +vt 1570.7963267949 -11.5 +vt 0 13 +vt 0 0.5 +vt 1570.7963267949 0.5 +vt 1570.7963267949 13 +vt 1570.7963267949 1570.7963267949 +vt 0 0 +vt 1570.7963267949 0 +vt 523.598775598299 -0.600860619027557 +vt 0 0 +vt 523.598775598299 -5.91462736163875E-02 +vt 1570.7963267949 -0.600860619027557 +vt 1047.1975511966 0 +vt 1570.7963267949 -5.91462736163877E-02 +vt 2617.99387799149 -0.600860619027557 +vt 2094.3951023932 0 +vt 2617.99387799149 -5.91462736163874E-02 +vt 3665.19142918809 -0.600860619027557 +vt 3141.59265358979 0 +vt 3665.19142918809 -5.91462736163873E-02 +vt 4712.38898038469 -0.600860619027557 +vt 4188.79020478639 0 +vt 4712.38898038469 -5.91462736163874E-02 +vt -523.598775598301 -0.600860619027557 +vt 5235.98775598299 0 +vt 5759.58653158129 -5.91462736163874E-02 +vt 0 -0.600860619027557 +vt 5235.98775598299 -0.600860619027557 +vt 4188.79020478639 -0.600860619027557 +vt 3141.59265358979 -0.600860619027557 +vt 2094.3951023932 -0.600860619027557 +vt 1047.1975511966 -0.600860619027557 +vt -523.5987755983 1.4328367550024E-15 +vt 0 1.4328367550024E-15 +vt 0 -4 +vt -1047.1975511966 1.4328367550024E-15 +vt 5759.58653158129 -4 +vt 4712.38898038469 1.4328367550024E-15 +vt -1047.1975511966 -4 +vt 4188.79020478639 1.4328367550024E-15 +vt 4712.38898038469 -4 +vt 3665.19142918809 1.4328367550024E-15 +vt 4188.79020478639 -4 +vt 3141.59265358979 1.4328367550024E-15 +vt 3665.19142918809 -4 +vt 2617.99387799149 1.4328367550024E-15 +vt 3141.59265358979 -4 +vt 2094.3951023932 1.4328367550024E-15 +vt 2617.99387799149 -4 +vt 1570.7963267949 1.4328367550024E-15 +vt 2094.3951023932 -4 +vt 1047.1975511966 1.4328367550024E-15 +vt 1570.7963267949 -4 +vt 523.598775598298 1.4328367550024E-15 +vt 1047.1975511966 -4 +vt 523.598775598298 -4 +vt -12 -0.5 +vt -4.36602540378443 -2.75000000000001 +vt -6.50000000000001 -5 +vt -4.49999999999999 -3.25 +vt -7.62500000000001 -5.30144284148501 +vt -8.44855715851499 -6.125 +vt -4 -2.38397459621557 +vt -4.55144284148502 -6.125 +vt -5.375 -5.30144284148502 +vt -4.25000000000001 -7.25 +vt -2 0 +vt -3.5 -2.25000000000001 +vt -3.00000000000001 -2.38397459621557 +vt -2.50000000000002 -3.25 +vt -2.63397459621558 -2.75 +vt -2 -14.5 +vt -2.63397459621557 -3.74999999999999 +vt -3.00000000000001 -4.11602540378443 +vt -2.50000000000001 -11.25 +vt -2.63397459621557 -11.75 +vt -2.63397459621557 -10.75 +vt -4.36602540378443 -3.74999999999999 +vt -6.50000000000001 -9.5 +vt -5.37500000000001 -9.19855715851499 +vt -4.36602540378443 -10.75 +vt -12 -14 +vt -7.62500000000001 -9.19855715851498 +vt -3.5 -4.24999999999999 +vt -4.55144284148502 -8.375 +vt -3.00000000000001 -10.3839745962156 +vt -4 -10.3839745962156 +vt -4.49999999999999 -11.25 +vt -8.75 -7.25 +vt -4 -4.11602540378443 +vt -3.5 -10.25 +vt -8.44855715851499 -8.375 +vt -3.99999999999999 -12.1160254037844 +vt -16.5 -14.5 +vt -17 -14 +vt -3.5 -12.25 +vt -3.00000000000001 -12.1160254037844 +vt -4.36602540378443 -11.75 +vt -16.5 0 +vt -17 -0.499999999999995 +vt -14.5 2 +vt 0 2 +vt 0 0.199999999999999 +vt -14.5 0.199999999999999 +vt -7.25 0.28284271247462 +vt -7.25 -2.88889491658085E-31 +vt 7.25 -2.88889491658085E-31 +vt 7.25 0.28284271247462 +vt 0 0 +vt 13.5 0 +vt 13.5 -5 +vt 0 -5 +vt 0 13.5 +vt 3 13.5 +vt 3 0 +vt 0 0 +vt 0 13.5 +vt 2 13.5 +vt 2 0 +vt 0 0 +vt 0 13.5 +vt 1.5 13.5 +vt 1.5 0 +vt 0 0 +vt 7.63844010796873E-17 0.707106781186548 +vt -33 0.707106781186548 +vt -32.5 0 +vt 7.63844010796873E-17 0 +vt 13 0.707106781186548 +vt 13.4662575430688 0 +vt 16.5 0 +vt 17 0.707106781186548 +vt 17.7580123735783 0.707106781186545 +vt 17.7580123735783 -1.62630325872826E-15 +vt 33.3601902741789 -2.16840434497101E-16 +vt 33.8264478172478 0.707106781186545 +vt 5307.73121899582 0 +vt 5760.55167804782 0 +vt 5760.55167804782 -0.5 +vt 6213.37213709982 0 +vt 6213.37213709982 -0.5 +vt 5307.73121899582 -0.5 +vt -9.43746139068091E-16 2.45326946669341E-15 +vt -9.43746139068091E-16 0.707106781186551 +vt 32.5 0.707106781186551 +vt 33 2.45326946669341E-15 +vt 4 2.45326946669341E-15 +vt 0 2.45326946669341E-15 +vt 0.5 0.707106781186551 +vt 3.53374245693117 0.707106781186551 +vt 16.0684354436694 -4.90653893338679E-15 +vt 0 -4.90653893338679E-15 +vt 0.466257543068835 0.70710678118654 +vt 16.0684354436694 0.70710678118654 +vt 3211.40582366956 0 +vt 3664.22628272156 0 +vt 3664.22628272156 -0.499999999999994 +vt 4117.04674177356 0 +vt 4117.04674177356 -0.499999999999994 +vt 3211.40582366956 -0.499999999999994 +vt -523.5987755983 0 +vt 0 0 +vt 0 -6.40000000000002 +vt -1047.1975511966 0 +vt -523.5987755983 -6.40000000000002 +vt 4712.38898038469 0 +vt -1047.1975511966 -6.40000000000002 +vt 4188.79020478639 0 +vt 4712.38898038469 -6.40000000000002 +vt 3665.19142918809 0 +vt 4188.79020478639 -6.40000000000002 +vt 3141.59265358979 0 +vt 3665.19142918809 -6.40000000000002 +vt 2617.99387799149 0 +vt 3141.59265358979 -6.40000000000002 +vt 2094.3951023932 0 +vt 2617.99387799149 -6.40000000000002 +vt 1570.7963267949 0 +vt 2094.3951023932 -6.40000000000002 +vt 1047.1975511966 0 +vt 1570.7963267949 -6.40000000000002 +vt 523.598775598299 0 +vt 1047.1975511966 -6.40000000000002 +vt 523.598775598299 -6.40000000000002 +vt -1.53073372946036 3.69551813004515 +vt 6.43249059870656E-16 4.00000000000001 +vt -6.37373125985397E-16 2.25 +vt 1.53073372946036 3.69551813004515 +vt 1.125 1.94855715851499 +vt 2.8284271247462 2.82842712474619 +vt 1.94855715851499 1.125 +vt 3.69551813004515 1.53073372946036 +vt 4.00000000000001 1.73472347597681E-15 +vt 2.25 8.67361737988404E-16 +vt 3.69551813004515 -1.53073372946036 +vt 1.94855715851499 -1.125 +vt 2.8284271247462 -2.82842712474619 +vt 1.125 -1.94855715851498 +vt 1.53073372946036 -3.69551813004515 +vt 7.34788079488413E-16 -4.00000000000001 +vt 2.41171973903751E-15 -2.25 +vt -2.82842712474619 2.82842712474619 +vt -1.125 1.94855715851499 +vt -3.69551813004515 1.53073372946036 +vt -1.94855715851499 1.125 +vt -2.25 0 +vt -1.53073372946036 -3.69551813004515 +vt -1.125 -1.94855715851499 +vt -2.82842712474619 -2.8284271247462 +vt -1.94855715851498 -1.125 +vt -3.69551813004515 -1.53073372946036 +vt -4.00000000000001 0 +vt 1570.7963267949 0 +vt 1767.14586764426 8.65980907535228 +vt 1963.49540849362 8.50563531497817 +vt 1570.7963267949 8.71186718344345 +vt 1963.49540849362 0 +vt 2159.84494934298 8.25527071347068 +vt 2356.19449019235 7.91833664205963 +vt 2356.19449019235 0 +vt 2552.54403104171 7.50778128815457 +vt 2748.89357189107 7.03938206376396 +vt 2748.89357189107 0 +vt 2945.24311274043 6.53113928839981 +vt 3141.59265358979 6.00258444689356 +vt 3141.59265358979 0 +vt 3337.94219443916 5.4740296053873 +vt 3534.29173528852 4.96578683002316 +vt 3534.29173528852 0 +vt 3730.64127613788 4.49738760563255 +vt 3926.99081698724 4.08683225172748 +vt 3926.99081698724 0 +vt 4123.3403578366 3.74989818031644 +vt 4319.68989868597 3.49953357880894 +vt 4712.38898038469 0 +vt 4319.68989868597 0 +vt 4516.03943953533 3.34535981843484 +vt 4712.38898038469 3.29330171034366 +vt 4908.73852123405 3.34535981843484 +vt 5105.08806208341 0 +vt -785.398163397448 0 +vt 5105.08806208341 3.49953357880894 +vt 5301.43760293278 3.74989818031644 +vt -392.699081698724 0 +vt -785.398163397448 4.08683225172748 +vt -589.048622548086 4.49738760563255 +vt 0 0 +vt -392.699081698724 4.96578683002316 +vt -196.349540849362 5.4740296053873 +vt 392.699081698724 0 +vt 0 6.00258444689356 +vt 196.349540849362 6.53113928839981 +vt 785.398163397448 0 +vt 392.699081698724 7.03938206376396 +vt 589.048622548086 7.50778128815457 +vt 1178.09724509617 0 +vt 785.398163397448 7.91833664205963 +vt 981.747704246811 8.25527071347068 +vt 1178.09724509617 8.50563531497817 +vt 1374.44678594553 8.65980907535228 +vt 5.54728128547267 0.707106781186545 +vt 5.54728128547267 0 +vt 17.7787953673403 0 +vt 17.7787953673403 0.707106781186545 +vt 16.5 8.41851098635803E-16 +vt 1.15881178091024 8.18099081691547 +vt 0.674899564441501 7.641984638772 +vt 0.499999999999997 6.93905588300604 +vt 11.2859748694946 15.0403278484472 +vt 16.5 32.5 +vt 13.4662575430688 32.5 +vt 12.3779046299945 16.9358282206026 +vt 12.0502634383153 15.8623095902267 +vt 2 0 +vt 0.499999999999997 -2 +vt 1.8 -2 +vt 2 -1.8 +vt -2 0.707106781186545 +vt -2 3.0044587332228E-16 +vt 6.93905588300604 3.0044587332228E-16 +vt 6.93905588300604 0.707106781186545 +vt 7.25 1.8 +vt -7.25 1.8 +vt -6.75 -1.22464679914736E-16 +vt 6.75000000000001 -9.23922836541318E-16 +vt -7.25 0.499999999999997 +vt 7.25 0.499999999999993 +vt 16.5 1.020425574104E-16 +vt 0.499999999999994 6.93905588300604 +vt 0.674899564441498 7.641984638772 +vt 2 1.020425574104E-16 +vt 16.5 32.5 +vt 12.3779046299945 16.9358282206026 +vt 1.15881178091024 8.18099081691546 +vt 2 -1.8 +vt 1.8 -2 +vt 12.0502634383153 15.8623095902267 +vt 11.2859748694946 15.0403278484472 +vt 0.499999999999993 -2 +vt 13.4662575430688 32.5 +vt 1.06094411699396 0.707106781186538 +vt 10 0.707106781186538 +vt 10 -6.01853107621011E-33 +vt 1.06094411699396 -6.01853107621011E-33 +vt 0.973076823710508 0.707106781186541 +vt 13.2045909055782 0.707106781186541 +vt 13.2045909055782 -4.68969849888969E-15 +vt 0.973076823710508 -4.68969849888969E-15 +vt -10 -0.5 +vt -1.06094411699396 -0.5 +vt -1.06094411699396 -14 +vt -10 -14 +vt 0 14 +vt 0 0.5 +vt 487.727044091881 0.5 +vt 487.727044091881 14 +vt 975.454088183763 0.5 +vt 975.454088183763 14 +vt 0 0.5 +vt 0 0 +vt 487.727044091882 0 +vt 487.727044091882 0.5 +vt 975.454088183763 0 +vt 975.454088183763 0.5 +vt 2166.13856540603 0.499999999999991 +vt 2166.13856540603 4.33680868994201E-16 +vt 2653.86560949791 4.33680868994201E-16 +vt 2653.86560949791 0.499999999999991 +vt 3141.59265358979 4.33680868994201E-16 +vt 3141.59265358979 0.499999999999991 +vt -13.2045909055782 -0.500000000000001 +vt -0.973076823710508 -0.500000000000001 +vt -6.41488219544817 -3.24999999999999 +vt -13.2045909055782 -14 +vt -7.35739689580537 -11.1731411216129 +vt -6.41488219544817 -11.25 +vt -5.47236749509098 -3.32685887838707 +vt -0.973076823710507 -14 +vt -4.56607310610018 -3.55448186995485 +vt -5.47236749509098 -11.1731411216129 +vt -7.35739689580537 -3.32685887838707 +vt -8.26369128479617 -10.9455181300452 +vt -8.26369128479617 -3.55448186995485 +vt -4.56607310610018 -10.9455181300452 +vt -3.73082741358894 -3.92412155078981 +vt -2.99872844110929 -4.42157287525381 +vt -9.09893697730741 -10.5758784492102 +vt 0.635468647529192 -0.500000000000001 +vt -2.99872844110929 -10.0784271247462 +vt 0.635468647529192 -14 +vt -9.83103594978706 -10.0784271247462 +vt -9.09893697730741 -3.92412155078981 +vt -3.73082741358894 -10.5758784492102 +vt -2.39791034160778 -5.02771906792159 +vt -9.83103594978706 -4.42157287525381 +vt -10.4318540492886 -9.47228093207841 +vt -1.95146221770559 -5.71926627053964 +vt -2.39791034160778 -9.47228093207841 +vt -10.4318540492886 -5.02771906792159 +vt -10.8783021731908 -8.78073372946036 +vt -10.8783021731908 -5.71926627053964 +vt -11.1532235704631 -8.03036128806452 +vt -1.67654082043329 -6.46963871193549 +vt -1.95146221770559 -8.78073372946037 +vt -11.246053165986 -7.25 +vt -11.1532235704631 -6.46963871193549 +vt -1.67654082043329 -8.03036128806452 +vt -1.58371122491035 -7.25 +vt -13.4662575430688 32.5 +vt -13 33 +vt -11.8791226048646 16.9707064574746 +vt -12.3373846721409 16.3563658266016 +vt -17 -7.63844010796873E-17 +vt -16.5 3 +vt -15 3 +vt -17 33 +vt -16.5 32.5 +vt -11.6170096515213 16.1118915531739 +vt -11.0055787964646 15.4543061597503 +vt -12 5 +vt -12 6.12255344462402E-16 +vt -15 5 +vt -13.4662575430688 32.5 +vt -12.3373846721409 16.3563658266016 +vt -11.8791226048646 16.9707064574746 +vt -11.6170096515213 16.1118915531739 +vt -11.0055787964646 15.4543061597503 +vt -13 33 +vt -17 33 +vt -16.5 32.5 +vt -17 -7.63844010796873E-17 +vt -16.5 3 +vt -12 0 +vt -15 3 +vt -12 5 +vt -15 5 +vt -13.5 0 +vt 0 0 +vt 0 -3.03374245693117 +vt -13.5 -3.03374245693117 +vt 13.5 29.5 +vt 9.25 16.9999999999999 +vt 8.96364006413302 18.1618079301093 +vt 0 0 +vt 5.86348778239366 14.6624593932864 +vt 7.05134170063831 14.5182278147548 +vt 8.17016186682789 19.057459664734 +vt 13.5 0 +vt 8.96364006413303 15.8381920698905 +vt 4.87872312957225 15.3421933543979 +vt 0 29.5 +vt 7.05134170063831 19.481772185245 +vt 8.17016186682789 14.9425403352658 +vt 4.32264545643487 16.401710839281 +vt 4.32264545643487 17.5982891607188 +vt 5.86348778239366 19.3375406067134 +vt 4.87872312957225 18.6578066456019 +vt -16.1830552827095 13.5 +vt 0 13.5 +vt -11.9307736041423 9.17735454356513 +vt -12.5305237260147 9.25 +vt -11.3658787793501 8.96364006413303 +vt -16.1830552827095 0 +vt -12.5305237260147 4.25 +vt -11.9307736041423 4.32264545643487 +vt -13.1302738478872 9.17735454356513 +vt -13.1302738478872 4.32264545643487 +vt -13.6951686726794 8.96364006413303 +vt 0 0 +vt -10.8686688855459 8.62127687042775 +vt -13.6951686726794 4.53635993586698 +vt -14.1923785664836 8.62127687042775 +vt -14.1923785664836 4.87872312957225 +vt -10.4680399544732 8.17016186682789 +vt -11.3658787793501 4.53635993586698 +vt -14.5930074975563 8.17016186682789 +vt -10.8686688855459 4.87872312957225 +vt -15.0366284712177 6.75 +vt -14.995578242291 6.29936240546524 +vt -15.0183561459969 7.05134170063831 +vt -10.187275083379 7.63651221760634 +vt -14.5930074975563 5.32983813317211 +vt -14.8737723686505 5.86348778239366 +vt -14.8737723686505 7.63651221760634 +vt -10.0426913060326 7.05134170063831 +vt -10.0426913060326 6.44865829936169 +vt -10.4680399544732 5.32983813317211 +vt -10.187275083379 5.86348778239366 +vt 5316.54141376734 0.5 +vt 4833.21946706122 0.5 +vt 5074.88044041428 4.57128436864092 +vt 4833.21946706122 4.58137002310922 +vt -483.321946706122 0.5 +vt 5558.2023871204 4.52375273977066 +vt 5316.54141376734 4.55169920128217 +vt 0 0.5 +vt -241.660973353061 4.44966405985439 +vt 5799.86336047347 4.4890691308682 +vt 483.321946706122 0.5 +vt 241.660973353061 4.36599115259554 +vt 0 4.40782760622496 +vt 7249.82920059183 0.5 +vt 7008.16822723877 4.29190247267926 +vt 6766.50725388571 4.32658608158173 +vt 7733.15114729795 0.5 +vt 7491.49017394489 4.244370843809 +vt 7249.82920059183 4.26395601116776 +vt 7733.15114729795 4.23428518934071 +vt 7974.81212065101 4.23428518934071 +vt 8216.47309400407 0.5 +vt 8216.47309400407 4.244370843809 +vt 8458.13406735714 4.26395601116776 +vt 8699.7950407102 0.5 +vt 8699.7950407102 4.29190247267926 +vt 8941.45601406326 4.32658608158173 +vt 2899.93168023673 0.5 +vt 9183.11698741632 4.36599115259554 +vt 3141.59265358979 4.40782760622496 +vt 3383.25362694285 0.5 +vt 3383.25362694285 4.44966405985439 +vt 3624.91460029591 4.4890691308682 +vt 3866.57557364898 0.5 +vt 3866.57557364898 4.52375273977066 +vt 4108.23654700204 4.55169920128217 +vt 4349.8975203551 0.5 +vt 4531.14325036989 4.57978111686741 +vt 4349.8975203551 4.57128436864092 +vt 4712.38898038469 4.58264463608372 +vt -523.5987755983 1.4328367550024E-15 +vt 0 1.4328367550024E-15 +vt 0 -4 +vt -1047.1975511966 1.4328367550024E-15 +vt 5759.58653158129 -4 +vt 4712.38898038469 1.4328367550024E-15 +vt -1047.1975511966 -4 +vt 4188.79020478639 1.4328367550024E-15 +vt 4712.38898038469 -4 +vt 3665.19142918809 1.4328367550024E-15 +vt 4188.79020478639 -4 +vt 3141.59265358979 1.4328367550024E-15 +vt 3665.19142918809 -4 +vt 2617.99387799149 1.4328367550024E-15 +vt 3141.59265358979 -4 +vt 2094.3951023932 1.4328367550024E-15 +vt 2617.99387799149 -4 +vt 1570.7963267949 1.4328367550024E-15 +vt 2094.3951023932 -4 +vt 1047.1975511966 1.4328367550024E-15 +vt 1570.7963267949 -4 +vt 523.598775598298 1.4328367550024E-15 +vt 1047.1975511966 -4 +vt 523.598775598298 -4 +vt 523.598775598299 -0.600860619027557 +vt 0 0 +vt 523.598775598299 -5.91462736163875E-02 +vt 1570.7963267949 -0.600860619027557 +vt 1047.1975511966 0 +vt 1570.7963267949 -5.91462736163877E-02 +vt 2617.99387799149 -0.600860619027557 +vt 2094.3951023932 0 +vt 2617.99387799149 -5.91462736163873E-02 +vt 3665.19142918809 -0.600860619027557 +vt 3141.59265358979 0 +vt 3665.19142918809 -5.91462736163876E-02 +vt 4712.38898038469 -0.600860619027557 +vt 4188.79020478639 0 +vt 4712.38898038469 -5.91462736163874E-02 +vt -523.598775598301 -0.600860619027557 +vt 5235.98775598299 0 +vt 5759.58653158129 -5.91462736163874E-02 +vt 0 -0.600860619027557 +vt 5235.98775598299 -0.600860619027557 +vt 4188.79020478639 -0.600860619027557 +vt 3141.59265358979 -0.600860619027557 +vt 2094.3951023932 -0.600860619027557 +vt 1047.1975511966 -0.600860619027557 +# 777 UVs + +g mesh1 +usemtl mat1 +f 3/3/3 1/1/1 2/2/2 +f 1/1/1 3/3/3 4/4/4 +f 1/1/1 4/4/4 5/5/5 +f 1/1/1 5/5/5 6/6/6 +f 1/1/1 6/6/6 7/7/7 +f 1/1/1 7/7/7 8/8/8 +f 1/1/1 8/8/8 9/9/9 +f 1/1/1 9/9/9 10/10/10 +f 1/1/1 10/10/10 11/11/11 +f 1/1/1 11/11/11 12/12/12 +f 1/1/1 12/12/12 13/13/13 +f 1/1/1 13/13/13 14/14/14 +f 1/1/1 14/14/14 15/15/15 +f 1/1/1 15/15/15 16/16/16 +f 1/1/1 16/16/16 17/17/17 +f 1/1/1 17/17/17 18/18/18 +f 1/1/1 18/18/18 19/19/19 +f 1/1/1 19/19/19 20/20/20 +f 1/1/1 20/20/20 21/21/21 +f 1/1/1 21/21/21 22/22/22 +f 1/1/1 22/22/22 23/23/23 +f 1/1/1 23/23/23 24/24/24 +f 1/1/1 24/24/24 25/25/25 +f 1/1/1 25/25/25 26/26/26 +f 1/1/1 26/26/26 27/27/27 +f 1/1/1 27/27/27 28/28/28 +f 1/1/1 28/28/28 29/29/29 +f 1/1/1 29/29/29 30/30/30 +f 1/1/1 30/30/30 31/31/31 +f 1/1/1 31/31/31 2/2/2 +f 34/34/34 32/32/32 33/33/33 +f 36/36/36 35/35/35 32/32/32 +f 38/38/38 37/37/37 35/35/35 +f 40/40/40 39/39/39 37/37/37 +f 42/42/42 41/41/41 39/39/39 +f 44/44/44 43/43/43 41/41/41 +f 46/46/46 45/45/45 43/43/43 +f 45/45/45 47/47/47 48/48/48 +f 47/47/47 49/49/49 48/48/48 +f 51/51/51 50/50/50 49/49/49 +f 53/53/53 52/52/52 50/50/50 +f 55/55/55 54/54/54 52/52/52 +f 57/57/57 56/56/56 54/54/54 +f 59/59/59 58/58/58 56/56/56 +f 61/61/61 60/60/60 58/58/58 +f 63/63/63 60/60/60 62/62/62 +f 65/65/65 64/64/64 63/63/63 +f 67/67/67 66/66/66 65/65/65 +f 69/69/69 68/68/68 67/67/67 +f 71/71/71 70/70/70 69/69/69 +f 73/73/73 72/72/72 71/71/71 +f 75/75/75 74/74/74 73/73/73 +f 77/77/77 76/76/76 75/75/75 +f 79/79/79 78/78/78 77/77/77 +f 81/81/81 80/80/80 79/79/79 +f 83/83/83 82/82/82 81/81/81 +f 85/85/85 84/84/84 83/83/83 +f 87/87/87 86/86/86 85/85/85 +f 89/89/89 88/88/88 87/87/87 +f 91/91/91 33/33/33 90/90/90 +f 91/91/91 90/90/90 89/89/89 +f 33/33/33 91/91/91 34/34/34 +f 88/88/88 89/89/89 90/90/90 +f 86/86/86 87/87/87 88/88/88 +f 84/84/84 85/85/85 86/86/86 +f 82/82/82 83/83/83 84/84/84 +f 80/80/80 81/81/81 82/82/82 +f 78/78/78 79/79/79 80/80/80 +f 76/76/76 77/77/77 78/78/78 +f 74/74/74 75/75/75 76/76/76 +f 72/72/72 73/73/73 74/74/74 +f 70/70/70 71/71/71 72/72/72 +f 68/68/68 69/69/69 70/70/70 +f 66/66/66 67/67/67 68/68/68 +f 64/64/64 65/65/65 66/66/66 +f 60/60/60 63/63/63 64/64/64 +f 60/60/60 61/61/61 62/62/62 +f 58/58/58 59/59/59 61/61/61 +f 56/56/56 57/57/57 59/59/59 +f 54/54/54 55/55/55 57/57/57 +f 52/52/52 53/53/53 55/55/55 +f 50/50/50 51/51/51 53/53/53 +f 49/49/49 47/47/47 51/51/51 +f 45/45/45 46/46/46 47/47/47 +f 43/43/43 44/44/44 46/46/46 +f 41/41/41 42/42/42 44/44/44 +f 39/39/39 40/40/40 42/42/42 +f 37/37/37 38/38/38 40/40/40 +f 35/35/35 36/36/36 38/38/38 +f 32/32/32 34/34/34 36/36/36 +f 94/94/94 92/92/92 93/93/93 +f 96/96/96 92/92/92 95/95/95 +f 99/99/99 97/97/97 98/98/98 +f 100/100/100 97/97/97 92/92/92 +f 93/93/93 92/92/92 101/101/101 +f 92/92/92 96/96/96 102/102/102 +f 98/98/98 97/97/97 103/103/103 +f 97/97/97 100/100/100 104/104/104 +f 101/101/101 92/92/92 105/105/105 +f 92/92/92 102/102/102 106/106/106 +f 103/103/103 97/97/97 107/107/107 +f 97/97/97 104/104/104 108/108/108 +f 105/105/105 92/92/92 109/109/109 +f 92/92/92 106/106/106 110/110/110 +f 107/107/107 97/97/97 111/111/111 +f 97/97/97 108/108/108 112/112/112 +f 109/109/109 92/92/92 113/113/113 +f 92/92/92 110/110/110 114/114/114 +f 111/111/111 97/97/97 115/115/115 +f 97/97/97 112/112/112 116/116/116 +f 113/113/113 92/92/92 117/117/117 +f 92/92/92 114/114/114 118/118/118 +f 119/119/119 92/92/92 97/97/97 +f 97/97/97 116/116/116 115/115/115 +f 92/92/92 118/118/118 117/117/117 +f 97/97/97 99/99/99 120/120/120 +f 92/92/92 119/119/119 95/95/95 +f 92/92/92 94/94/94 121/121/121 +f 224/224/224 226/226/226 227/227/227 +f 226/226/226 224/224/224 225/225/225 +f 228/228/228 230/230/230 231/231/231 +f 230/230/230 228/228/228 229/229/229 +f 204/204/204 203/203/203 206/206/206 +f 203/203/203 205/205/205 207/207/207 +f 206/206/206 203/203/203 208/208/208 +f 205/205/205 203/203/203 204/204/204 +f 169/169/169 171/171/171 172/172/172 +f 171/171/171 169/169/169 170/170/170 +f 165/165/165 167/167/167 168/168/168 +f 167/167/167 165/165/165 166/166/166 +f 163/163/163 161/161/161 162/162/162 +f 161/161/161 163/163/163 164/164/164 +f 157/157/157 159/159/159 160/160/160 +f 159/159/159 157/157/157 158/158/158 +f 155/155/155 153/153/153 154/154/154 +f 153/153/153 155/155/155 156/156/156 +f 151/151/151 149/149/149 150/150/150 +f 149/149/149 151/151/151 152/152/152 +f 147/147/147 145/145/145 146/146/146 +f 145/145/145 147/147/147 148/148/148 +f 140/140/140 139/139/139 138/138/138 +f 139/139/139 141/141/141 143/143/143 +f 135/135/135 137/137/137 142/142/142 +f 138/138/138 142/142/142 144/144/144 +f 138/138/138 135/135/135 142/142/142 +f 141/141/141 139/139/139 140/140/140 +f 137/137/137 135/135/135 136/136/136 +f 135/135/135 138/138/138 139/139/139 +f 133/133/133 131/131/131 132/132/132 +f 131/131/131 133/133/133 134/134/134 +f 175/175/175 173/173/173 174/174/174 +f 177/177/177 174/174/174 176/176/176 +f 174/174/174 177/177/177 175/175/175 +f 173/173/173 175/175/175 184/184/184 +f 182/182/182 181/181/181 180/180/180 +f 181/181/181 182/182/182 183/183/183 +f 180/180/180 179/179/179 178/178/178 +f 179/179/179 180/180/180 181/181/181 +f 176/176/176 179/179/179 177/177/177 +f 179/179/179 176/176/176 178/178/178 +f 211/211/211 209/209/209 210/210/210 +f 213/213/213 209/209/209 212/212/212 +f 214/214/214 213/213/213 212/212/212 +f 215/215/215 216/216/216 217/217/217 +f 209/209/209 213/213/213 210/210/210 +f 216/216/216 215/215/215 214/214/214 +f 218/218/218 211/211/211 220/220/220 +f 209/209/209 211/211/211 219/219/219 +f 213/213/213 214/214/214 215/215/215 +f 211/211/211 218/218/218 219/219/219 + +g mesh2 +usemtl mat2 +f 127/127/127 125/125/125 126/126/126 +f 129/129/129 125/125/125 128/128/128 +f 125/125/125 129/129/129 126/126/126 +f 125/125/125 127/127/127 130/130/130 +f 189/189/189 185/185/185 188/188/188 +f 185/185/185 187/187/187 190/190/190 +f 185/185/185 189/189/189 186/186/186 +f 187/187/187 185/185/185 186/186/186 +f 193/193/193 191/191/191 192/192/192 +f 191/191/191 193/193/193 194/194/194 +f 124/124/124 122/122/122 123/123/123 +f 223/223/223 221/221/221 222/222/222 +f 195/195/195 197/197/197 198/198/198 +f 197/197/197 195/195/195 196/196/196 +f 201/201/201 199/199/199 200/200/200 +f 199/199/199 201/201/201 202/202/202 +f 235/235/235 237/237/237 238/238/238 +f 237/237/237 235/235/235 236/236/236 +f 241/241/241 239/239/239 240/240/240 +f 239/239/239 241/241/241 242/242/242 +f 234/234/234 232/232/232 233/233/233 +f 249/249/249 247/247/247 248/248/248 +f 245/245/245 243/243/243 244/244/244 +f 243/243/243 245/245/245 246/246/246 + +g mesh3 +usemtl mat3 +f 313/313/313 334/334/334 337/337/337 +f 321/321/321 328/328/328 322/322/322 +f 340/340/340 298/298/298 341/341/341 +f 313/313/313 338/338/338 317/317/317 +f 298/298/298 340/340/340 308/308/308 +f 339/339/339 323/323/323 329/329/329 +f 334/334/334 323/323/323 339/339/339 +f 338/338/338 313/313/313 337/337/337 +f 298/298/298 303/303/303 330/330/330 +f 335/335/335 323/323/323 313/313/313 +f 323/323/323 333/333/333 324/324/324 +f 323/323/323 335/335/335 336/336/336 +f 334/334/334 313/313/313 323/323/323 +f 326/326/326 332/332/332 328/328/328 +f 316/316/316 331/331/331 325/325/325 +f 323/323/323 330/330/330 333/333/333 +f 320/320/320 322/322/322 329/329/329 +f 326/326/326 327/327/327 332/332/332 +f 300/300/300 298/298/298 299/299/299 +f 301/301/301 300/300/300 299/299/299 +f 303/303/303 298/298/298 302/302/302 +f 299/299/299 298/298/298 304/304/304 +f 306/306/306 301/301/301 305/305/305 +f 298/298/298 300/300/300 302/302/302 +f 305/305/305 301/301/301 307/307/307 +f 304/304/304 308/308/308 309/309/309 +f 300/300/300 301/301/301 306/306/306 +f 309/309/309 308/308/308 310/310/310 +f 312/312/312 308/308/308 311/311/311 +f 311/311/311 313/313/313 314/314/314 +f 316/316/316 315/315/315 313/313/313 +f 308/308/308 304/304/304 298/298/298 +f 313/313/313 311/311/311 308/308/308 +f 316/316/316 313/313/313 317/317/317 +f 319/319/319 318/318/318 301/301/301 +f 322/322/322 320/320/320 321/321/321 +f 320/320/320 323/323/323 324/324/324 +f 308/308/308 312/312/312 310/310/310 +f 315/315/315 316/316/316 325/325/325 +f 327/327/327 326/326/326 318/318/318 +f 318/318/318 319/319/319 316/316/316 +f 301/301/301 318/318/318 307/307/307 +f 328/328/328 321/321/321 326/326/326 +f 323/323/323 320/320/320 329/329/329 +f 330/330/330 323/323/323 298/298/298 +f 313/313/313 315/315/315 314/314/314 +f 331/331/331 316/316/316 319/319/319 +f 307/307/307 318/318/318 326/326/326 +f 592/592/592 575/575/575 597/597/597 +f 601/601/601 575/575/575 603/603/603 +f 598/598/598 589/589/589 604/604/604 +f 591/591/591 599/599/599 605/605/605 +f 606/606/606 575/575/575 572/572/572 +f 572/572/572 602/602/602 607/607/607 +f 609/609/609 591/591/591 608/608/608 +f 575/575/575 606/606/606 603/603/603 +f 604/604/604 589/589/589 609/609/609 +f 591/591/591 605/605/605 608/608/608 +f 591/591/591 609/609/609 589/589/589 +f 572/572/572 607/607/607 606/606/606 +f 572/572/572 574/574/574 582/582/582 +f 579/579/579 577/577/577 581/581/581 +f 578/578/578 573/573/573 580/580/580 +f 575/575/575 577/577/577 579/579/579 +f 574/574/574 573/573/573 578/578/578 +f 577/577/577 575/575/575 576/576/576 +f 597/597/597 575/575/575 601/601/601 +f 572/572/572 596/596/596 600/600/600 +f 574/574/574 572/572/572 573/573/573 +f 572/572/572 600/600/600 602/602/602 +f 591/591/591 590/590/590 599/599/599 +f 595/595/595 589/589/589 598/598/598 +f 576/576/576 575/575/575 583/583/583 +f 572/572/572 593/593/593 596/596/596 +f 590/590/590 579/579/579 594/594/594 +f 587/587/587 589/589/589 595/595/595 +f 579/579/579 585/585/585 594/594/594 +f 572/572/572 584/584/584 593/593/593 +f 588/588/588 575/575/575 592/592/592 +f 579/579/579 590/590/590 591/591/591 +f 587/587/587 573/573/573 589/589/589 +f 583/583/583 575/575/575 588/588/588 +f 573/573/573 587/587/587 586/586/586 +f 580/580/580 573/573/573 586/586/586 +f 579/579/579 581/581/581 585/585/585 +f 572/572/572 582/582/582 584/584/584 +f 550/550/550 552/552/552 553/553/553 +f 552/552/552 550/550/550 551/551/551 +f 506/506/506 509/509/509 515/515/515 +f 516/516/516 515/515/515 509/509/509 +f 506/506/506 513/513/513 514/514/514 +f 515/515/515 517/517/517 518/518/518 +f 517/517/517 515/515/515 516/516/516 +f 511/511/511 513/513/513 506/506/506 +f 510/510/510 506/506/506 514/514/514 +f 513/513/513 511/511/511 512/512/512 +f 507/507/507 506/506/506 510/510/510 +f 506/506/506 508/508/508 509/509/509 +f 508/508/508 506/506/506 507/507/507 +f 532/532/532 537/537/537 540/540/540 +f 532/532/532 540/540/540 530/530/530 +f 529/529/529 535/535/535 539/539/539 +f 534/534/534 529/529/529 538/538/538 +f 537/537/537 532/532/532 536/536/536 +f 529/529/529 531/531/531 535/535/535 +f 534/534/534 533/533/533 529/529/529 +f 530/530/530 529/529/529 532/532/532 +f 531/531/531 529/529/529 530/530/530 +f 533/533/533 534/534/534 541/541/541 +f 529/529/529 539/539/539 538/538/538 +f 556/556/556 554/554/554 555/555/555 +f 557/557/557 558/558/558 559/559/559 +f 554/554/554 556/556/556 557/557/557 +f 558/558/558 557/557/557 556/556/556 +f 468/468/468 467/467/467 466/466/466 +f 466/466/466 464/464/464 465/465/465 +f 462/462/462 461/461/461 460/460/460 +f 469/469/469 467/467/467 468/468/468 +f 465/465/465 464/464/464 463/463/463 +f 463/463/463 461/461/461 462/462/462 +f 472/472/472 470/470/470 471/471/471 +f 459/459/459 458/458/458 456/456/456 +f 455/455/455 454/454/454 457/457/457 +f 456/456/456 454/454/454 455/455/455 +f 475/475/475 473/473/473 474/474/474 +f 474/474/474 473/473/473 472/472/472 +f 478/478/478 476/476/476 477/477/477 +f 478/478/478 477/477/477 475/475/475 +f 480/480/480 476/476/476 479/479/479 +f 476/476/476 480/480/480 481/481/481 +f 483/483/483 482/482/482 481/481/481 +f 482/482/482 483/483/483 484/484/484 +f 486/486/486 485/485/485 482/482/482 +f 485/485/485 486/486/486 487/487/487 +f 460/460/460 458/458/458 459/459/459 +f 471/471/471 470/470/470 469/469/469 +f 454/454/454 456/456/456 458/458/458 +f 458/458/458 460/460/460 461/461/461 +f 461/461/461 463/463/463 464/464/464 +f 464/464/464 466/466/466 467/467/467 +f 467/467/467 469/469/469 470/470/470 +f 470/470/470 472/472/472 473/473/473 +f 473/473/473 475/475/475 477/477/477 +f 476/476/476 478/478/478 479/479/479 +f 481/481/481 480/480/480 483/483/483 +f 482/482/482 484/484/484 486/486/486 +f 485/485/485 487/487/487 489/489/489 +f 488/488/488 490/490/490 492/492/492 +f 491/491/491 493/493/493 495/495/495 +f 494/494/494 496/496/496 498/498/498 +f 497/497/497 499/499/499 500/500/500 +f 454/454/454 501/501/501 457/457/457 +f 454/454/454 500/500/500 501/501/501 +f 500/500/500 454/454/454 497/497/497 +f 497/497/497 498/498/498 499/499/499 +f 498/498/498 497/497/497 494/494/494 +f 494/494/494 495/495/495 496/496/496 +f 495/495/495 494/494/494 491/491/491 +f 491/491/491 492/492/492 493/493/493 +f 492/492/492 491/491/491 488/488/488 +f 488/488/488 489/489/489 490/490/490 +f 489/489/489 488/488/488 485/485/485 +f 445/445/445 447/447/447 453/453/453 +f 451/451/451 450/450/450 452/452/452 +f 452/452/452 447/447/447 451/451/451 +f 426/426/426 428/428/428 444/444/444 +f 447/447/447 452/452/452 453/453/453 +f 450/450/450 449/449/449 448/448/448 +f 448/448/448 442/442/442 441/441/441 +f 443/443/443 444/444/444 446/446/446 +f 449/449/449 450/450/450 451/451/451 +f 442/442/442 448/448/448 449/449/449 +f 439/439/439 438/438/438 440/440/440 +f 443/443/443 446/446/446 445/445/445 +f 447/447/447 445/445/445 446/446/446 +f 440/440/440 442/442/442 439/439/439 +f 433/433/433 435/435/435 432/432/432 +f 444/444/444 443/443/443 426/426/426 +f 442/442/442 440/440/440 441/441/441 +f 438/438/438 437/437/437 436/436/436 +f 436/436/436 435/435/435 434/434/434 +f 432/432/432 431/431/431 433/433/433 +f 437/437/437 438/438/438 439/439/439 +f 435/435/435 436/436/436 437/437/437 +f 435/435/435 433/433/433 434/434/434 +f 431/431/431 430/430/430 429/429/429 +f 429/429/429 428/428/428 427/427/427 +f 430/430/430 431/431/431 432/432/432 +f 428/428/428 429/429/429 430/430/430 +f 428/428/428 426/426/426 427/427/427 +f 402/402/402 404/404/404 406/406/406 +f 405/405/405 406/406/406 408/408/408 +f 407/407/407 408/408/408 410/410/410 +f 409/409/409 410/410/410 412/412/412 +f 411/411/411 412/412/412 414/414/414 +f 413/413/413 414/414/414 416/416/416 +f 415/415/415 416/416/416 418/418/418 +f 417/417/417 418/418/418 420/420/420 +f 419/419/419 420/420/420 422/422/422 +f 421/421/421 422/422/422 424/424/424 +f 423/423/423 424/424/424 425/425/425 +f 403/403/403 425/425/425 404/404/404 +f 425/425/425 403/403/403 423/423/423 +f 424/424/424 423/423/423 421/421/421 +f 422/422/422 421/421/421 419/419/419 +f 420/420/420 419/419/419 417/417/417 +f 418/418/418 417/417/417 415/415/415 +f 414/414/414 413/413/413 411/411/411 +f 404/404/404 402/402/402 403/403/403 +f 412/412/412 411/411/411 409/409/409 +f 410/410/410 409/409/409 407/407/407 +f 408/408/408 407/407/407 405/405/405 +f 406/406/406 405/405/405 402/402/402 +f 416/416/416 415/415/415 413/413/413 +f 273/273/273 252/252/252 254/254/254 +f 272/272/272 255/255/255 257/257/257 +f 271/271/271 258/258/258 260/260/260 +f 270/270/270 261/261/261 263/263/263 +f 269/269/269 264/264/264 266/266/266 +f 268/268/268 267/267/267 251/251/251 +f 267/267/267 265/265/265 266/266/266 +f 264/264/264 262/262/262 263/263/263 +f 261/261/261 259/259/259 260/260/260 +f 258/258/258 256/256/256 257/257/257 +f 255/255/255 253/253/253 254/254/254 +f 252/252/252 250/250/250 251/251/251 +f 276/276/276 274/274/274 275/275/275 +f 277/277/277 278/278/278 280/280/280 +f 279/279/279 280/280/280 282/282/282 +f 281/281/281 282/282/282 284/284/284 +f 283/283/283 284/284/284 286/286/286 +f 285/285/285 286/286/286 288/288/288 +f 287/287/287 288/288/288 290/290/290 +f 289/289/289 290/290/290 292/292/292 +f 291/291/291 292/292/292 294/294/294 +f 293/293/293 294/294/294 296/296/296 +f 295/295/295 296/296/296 297/297/297 +f 275/275/275 297/297/297 276/276/276 +f 278/278/278 277/277/277 274/274/274 +f 280/280/280 279/279/279 277/277/277 +f 282/282/282 281/281/281 279/279/279 +f 284/284/284 283/283/283 281/281/281 +f 286/286/286 285/285/285 283/283/283 +f 288/288/288 287/287/287 285/285/285 +f 290/290/290 289/289/289 287/287/287 +f 292/292/292 291/291/291 289/289/289 +f 294/294/294 293/293/293 291/291/291 +f 296/296/296 295/295/295 293/293/293 +f 297/297/297 275/275/275 295/295/295 +f 274/274/274 276/276/276 278/278/278 +f 759/759/759 757/757/757 758/758/758 +f 762/762/762 760/760/760 761/761/761 +f 765/765/765 763/763/763 764/764/764 +f 768/768/768 766/766/766 767/767/767 +f 771/771/771 769/769/769 770/770/770 +f 772/772/772 771/771/771 755/755/755 +f 773/773/773 768/768/768 770/770/770 +f 774/774/774 765/765/765 767/767/767 +f 775/775/775 762/762/762 764/764/764 +f 776/776/776 759/759/759 761/761/761 +f 756/756/756 754/754/754 755/755/755 +f 777/777/777 756/756/756 758/758/758 +f 748/748/748 747/747/747 745/745/745 +f 730/730/730 732/732/732 734/734/734 +f 733/733/733 734/734/734 736/736/736 +f 735/735/735 736/736/736 738/738/738 +f 737/737/737 738/738/738 740/740/740 +f 739/739/739 740/740/740 742/742/742 +f 741/741/741 742/742/742 744/744/744 +f 743/743/743 744/744/744 746/746/746 +f 745/745/745 746/746/746 748/748/748 +f 747/747/747 748/748/748 750/750/750 +f 734/734/734 733/733/733 730/730/730 +f 736/736/736 735/735/735 733/733/733 +f 738/738/738 737/737/737 735/735/735 +f 740/740/740 739/739/739 737/737/737 +f 742/742/742 741/741/741 739/739/739 +f 744/744/744 743/743/743 741/741/741 +f 746/746/746 745/745/745 743/743/743 +f 732/732/732 730/730/730 731/731/731 +f 750/750/750 749/749/749 747/747/747 +f 752/752/752 751/751/751 749/749/749 +f 753/753/753 731/731/731 751/751/751 +f 731/731/731 753/753/753 732/732/732 +f 751/751/751 752/752/752 753/753/753 +f 749/749/749 750/750/750 752/752/752 +f 342/342/342 344/344/344 345/345/345 +f 344/344/344 342/342/342 343/343/343 +f 525/525/525 524/524/524 527/527/527 +f 523/523/523 526/526/526 528/528/528 +f 525/525/525 523/523/523 524/524/524 +f 523/523/523 525/525/525 526/526/526 +f 348/348/348 346/346/346 347/347/347 +f 346/346/346 348/348/348 349/349/349 +f 610/610/610 612/612/612 613/613/613 +f 616/616/616 614/614/614 615/615/615 +f 615/615/615 617/617/617 618/618/618 +f 618/618/618 611/611/611 610/610/610 +f 613/613/613 612/612/612 619/619/619 +f 617/617/617 615/615/615 614/614/614 +f 613/613/613 619/619/619 620/620/620 +f 616/616/616 621/621/621 622/622/622 +f 611/611/611 618/618/618 617/617/617 +f 614/614/614 616/616/616 622/622/622 +f 621/621/621 616/616/616 623/623/623 +f 612/612/612 610/610/610 611/611/611 +f 630/630/630 633/633/633 631/631/631 +f 626/626/626 625/625/625 627/627/627 +f 626/626/626 624/624/624 625/625/625 +f 631/631/631 629/629/629 630/630/630 +f 629/629/629 631/631/631 624/624/624 +f 633/633/633 630/630/630 632/632/632 +f 632/632/632 635/635/635 633/633/633 +f 635/635/635 632/632/632 634/634/634 +f 627/627/627 625/625/625 628/628/628 +f 635/635/635 636/636/636 637/637/637 +f 636/636/636 635/635/635 634/634/634 +f 624/624/624 626/626/626 629/629/629 +f 352/352/352 350/350/350 351/351/351 +f 350/350/350 352/352/352 353/353/353 +f 354/354/354 356/356/356 357/357/357 +f 356/356/356 354/354/354 355/355/355 +f 358/358/358 360/360/360 361/361/361 +f 360/360/360 358/358/358 359/359/359 +f 362/362/362 364/364/364 365/365/365 +f 364/364/364 362/362/362 363/363/363 +f 649/649/649 654/654/654 650/650/650 +f 652/652/652 655/655/655 645/645/645 +f 652/652/652 657/657/657 658/658/658 +f 645/645/645 647/647/647 649/649/649 +f 642/642/642 648/648/648 653/653/653 +f 652/652/652 658/658/658 656/656/656 +f 651/651/651 645/645/645 655/655/655 +f 652/652/652 653/653/653 657/657/657 +f 649/649/649 643/643/643 642/642/642 +f 653/653/653 652/652/652 642/642/642 +f 646/646/646 645/645/645 651/651/651 +f 643/643/643 649/649/649 650/650/650 +f 642/642/642 644/644/644 648/648/648 +f 647/647/647 645/645/645 646/646/646 +f 644/644/644 642/642/642 643/643/643 +f 654/654/654 649/649/649 647/647/647 +f 655/655/655 652/652/652 656/656/656 +f 640/640/640 638/638/638 639/639/639 +f 638/638/638 640/640/640 641/641/641 +f 664/664/664 684/684/684 683/683/683 +f 659/659/659 677/677/677 685/685/685 +f 686/686/686 670/670/670 687/687/687 +f 670/670/670 678/678/678 688/688/688 +f 670/670/670 686/686/686 660/660/660 +f 670/670/670 688/688/688 689/689/689 +f 670/670/670 689/689/689 687/687/687 +f 659/659/659 679/679/679 664/664/664 +f 682/682/682 660/660/660 686/686/686 +f 681/681/681 659/659/659 685/685/685 +f 664/664/664 680/680/680 684/684/684 +f 674/674/674 664/664/664 683/683/683 +f 675/675/675 660/660/660 682/682/682 +f 679/679/679 659/659/659 681/681/681 +f 680/680/680 664/664/664 679/679/679 +f 670/670/670 676/676/676 678/678/678 +f 659/659/659 673/673/673 677/677/677 +f 670/670/670 666/666/666 676/676/676 +f 671/671/671 660/660/660 675/675/675 +f 672/672/672 664/664/664 674/674/674 +f 659/659/659 669/669/669 673/673/673 +f 668/668/668 664/664/664 672/672/672 +f 663/663/663 660/660/660 671/671/671 +f 664/664/664 666/666/666 670/670/670 +f 659/659/659 667/667/667 669/669/669 +f 665/665/665 664/664/664 668/668/668 +f 659/659/659 662/662/662 667/667/667 +f 666/666/666 664/664/664 665/665/665 +f 661/661/661 660/660/660 663/663/663 +f 659/659/659 661/661/661 662/662/662 +f 661/661/661 659/659/659 660/660/660 +f 692/692/692 690/690/690 691/691/691 +f 692/692/692 691/691/691 693/693/693 +f 695/695/695 694/694/694 690/690/690 +f 695/695/695 690/690/690 696/696/696 +f 698/698/698 697/697/697 694/694/694 +f 698/698/698 694/694/694 699/699/699 +f 701/701/701 700/700/700 697/697/697 +f 701/701/701 697/697/697 702/702/702 +f 704/704/704 703/703/703 700/700/700 +f 704/704/704 700/700/700 705/705/705 +f 707/707/707 706/706/706 703/703/703 +f 707/707/707 703/703/703 708/708/708 +f 710/710/710 706/706/706 709/709/709 +f 706/706/706 710/710/710 711/711/711 +f 713/713/713 711/711/711 712/712/712 +f 711/711/711 713/713/713 714/714/714 +f 716/716/716 714/714/714 715/715/715 +f 714/714/714 716/716/716 717/717/717 +f 719/719/719 717/717/717 718/718/718 +f 717/717/717 719/719/719 720/720/720 +f 722/722/722 720/720/720 721/721/721 +f 720/720/720 722/722/722 723/723/723 +f 725/725/725 723/723/723 724/724/724 +f 723/723/723 725/725/725 726/726/726 +f 727/727/727 691/691/691 726/726/726 +f 727/727/727 726/726/726 728/728/728 +f 691/691/691 727/727/727 729/729/729 +f 691/691/691 729/729/729 693/693/693 +f 726/726/726 725/725/725 728/728/728 +f 723/723/723 722/722/722 724/724/724 +f 720/720/720 719/719/719 721/721/721 +f 717/717/717 716/716/716 718/718/718 +f 714/714/714 713/713/713 715/715/715 +f 711/711/711 710/710/710 712/712/712 +f 706/706/706 707/707/707 709/709/709 +f 703/703/703 704/704/704 708/708/708 +f 700/700/700 701/701/701 705/705/705 +f 697/697/697 698/698/698 702/702/702 +f 694/694/694 695/695/695 699/699/699 +f 690/690/690 692/692/692 696/696/696 +f 521/521/521 519/519/519 520/520/520 +f 519/519/519 521/521/521 522/522/522 +f 560/560/560 562/562/562 563/563/563 +f 563/563/563 564/564/564 565/565/565 +f 564/564/564 563/563/563 562/562/562 +f 562/562/562 560/560/560 561/561/561 +f 502/502/502 504/504/504 505/505/505 +f 504/504/504 502/502/502 503/503/503 +f 378/378/378 380/380/380 383/383/383 +f 382/382/382 379/379/379 381/381/381 +f 379/379/379 382/382/382 380/380/380 +f 380/380/380 378/378/378 379/379/379 +f 376/376/376 374/374/374 375/375/375 +f 374/374/374 376/376/376 377/377/377 +f 372/372/372 370/370/370 371/371/371 +f 370/370/370 372/372/372 373/373/373 +f 366/366/366 368/368/368 369/369/369 +f 368/368/368 366/366/366 367/367/367 +f 542/542/542 544/544/544 545/545/545 +f 544/544/544 542/542/542 543/543/543 +f 568/568/568 566/566/566 567/567/567 +f 566/566/566 568/568/568 569/569/569 +f 569/569/569 570/570/570 571/571/571 +f 570/570/570 569/569/569 568/568/568 +f 548/548/548 546/546/546 547/547/547 +f 546/546/546 548/548/548 549/549/549 +f 396/396/396 398/398/398 401/401/401 +f 397/397/397 400/400/400 398/398/398 +f 398/398/398 396/396/396 397/397/397 +f 400/400/400 397/397/397 399/399/399 +f 394/394/394 392/392/392 393/393/393 +f 392/392/392 394/394/394 395/395/395 +f 388/388/388 390/390/390 391/391/391 +f 390/390/390 388/388/388 389/389/389 +f 386/386/386 384/384/384 385/385/385 +f 384/384/384 386/386/386 387/387/387 diff --git a/flexiv_description/meshes/grav/visual/inner_bar.stl b/flexiv_description/meshes/grav/visual/inner_bar.stl new file mode 100644 index 0000000..4b15f4b Binary files /dev/null and b/flexiv_description/meshes/grav/visual/inner_bar.stl differ diff --git a/flexiv_description/meshes/grav/visual/outer_bar.stl b/flexiv_description/meshes/grav/visual/outer_bar.stl new file mode 100644 index 0000000..30e015d Binary files /dev/null and b/flexiv_description/meshes/grav/visual/outer_bar.stl differ diff --git a/flexiv_description/meshes/grav/visual/static_assembly.stl b/flexiv_description/meshes/grav/visual/static_assembly.stl new file mode 100644 index 0000000..616e598 Binary files /dev/null and b/flexiv_description/meshes/grav/visual/static_assembly.stl differ diff --git a/flexiv_description/package.xml b/flexiv_description/package.xml index 3f06ace..af18dae 100644 --- a/flexiv_description/package.xml +++ b/flexiv_description/package.xml @@ -2,7 +2,7 @@ flexiv_description - 1.4.0 + 1.5.0 URDF description for Flexiv robots Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_description/urdf/grav_macro.xacro b/flexiv_description/urdf/grav_macro.xacro new file mode 100644 index 0000000..7fa53e6 --- /dev/null +++ b/flexiv_description/urdf/grav_macro.xacro @@ -0,0 +1,349 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flexiv_description/urdf/rizon.urdf.xacro b/flexiv_description/urdf/rizon.urdf.xacro index fdc8495..c66eff2 100644 --- a/flexiv_description/urdf/rizon.urdf.xacro +++ b/flexiv_description/urdf/rizon.urdf.xacro @@ -13,6 +13,7 @@ + @@ -24,6 +25,7 @@ rizon_type="$(arg rizon_type)" prefix="$(arg prefix)" robot_sn="$(arg robot_sn)" + load_gripper="$(arg load_gripper)" use_fake_hardware="$(arg use_fake_hardware)" fake_sensor_commands="$(arg fake_sensor_commands)" initial_positions="${xacro.load_yaml(initial_positions_file)}"> diff --git a/flexiv_description/urdf/rizon_macro.xacro b/flexiv_description/urdf/rizon_macro.xacro index b787f08..760c829 100644 --- a/flexiv_description/urdf/rizon_macro.xacro +++ b/flexiv_description/urdf/rizon_macro.xacro @@ -5,11 +5,18 @@ rizon_type prefix robot_sn + load_gripper:=false use_fake_hardware:=false fake_sensor_commands:=false initial_positions:=${dict(joint1=0.0,joint2=-0.69813,joint3=0.0,joint4=1.570796,joint5=0.0,joint6=0.69813,joint7=0.0)} "> + + + + + + diff --git a/flexiv_gripper/CMakeLists.txt b/flexiv_gripper/CMakeLists.txt new file mode 100644 index 0000000..00c290f --- /dev/null +++ b/flexiv_gripper/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.5) +project(flexiv_gripper) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(flexiv_hardware REQUIRED) +find_package(flexiv_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +add_library(gripper_action_server + SHARED + src/gripper_action_server.cpp) + +target_include_directories(gripper_action_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/include) + +# Set static library +message("OS: ${CMAKE_SYSTEM_NAME}") +message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a") + elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a") + else() + message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/libflexiv_rdk.arm64-darwin.a") + else() + message(FATAL_ERROR "Mac with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") + if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../flexiv_hardware/rdk/lib/flexiv_rdk.win_amd64.lib") + else() + message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") + endif() +endif() + +target_link_libraries(gripper_action_server + ${RDK_STATIC_LIBRARY}) + +ament_target_dependencies(gripper_action_server + control_msgs + flexiv_hardware + flexiv_msgs + rclcpp + rclcpp_action + rclcpp_components + sensor_msgs + std_srvs) + +rclcpp_components_register_node(gripper_action_server + PLUGIN "flexiv_gripper::GripperActionServer" + EXECUTABLE flexiv_gripper_node) + +install(TARGETS gripper_action_server + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}/) + +ament_package() diff --git a/flexiv_gripper/config/flexiv_gripper_node.yaml b/flexiv_gripper/config/flexiv_gripper_node.yaml new file mode 100644 index 0000000..26a7f1e --- /dev/null +++ b/flexiv_gripper/config/flexiv_gripper_node.yaml @@ -0,0 +1,6 @@ +flexiv_gripper: + ros__parameters: + state_publish_rate: 50 # Hz + feedback_publish_rate: 30 # Hz + default_velocity: 0.1 # m/s + default_max_force: 20.0 # N diff --git a/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp b/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp new file mode 100644 index 0000000..03cf8e6 --- /dev/null +++ b/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp @@ -0,0 +1,259 @@ +/** + * @file gripper_action_server.hpp + * @brief Header file for GripperActionServer class + * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved. + */ + +#ifndef FLEXIV_GRIPPER__GRIPPER_ACTION_SERVER_HPP_ +#define FLEXIV_GRIPPER__GRIPPER_ACTION_SERVER_HPP_ + +#include +#include +#include +#include +#include +#include + +// ROS +#include "control_msgs/action/gripper_command.hpp" +#include "flexiv_msgs/action/grasp.hpp" +#include "flexiv_msgs/action/move.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_srvs/srv/trigger.hpp" + +// Flexiv +#include "flexiv/rdk/data.hpp" +#include "flexiv/rdk/gripper.hpp" +#include "flexiv/rdk/robot.hpp" + +namespace { + +const int kDefaultStatePublishRate = 30; // [Hz] +const int kDefaultFeedbackPublishRate = 10; // [Hz] +const double kDefaultVelocity = 0.1; // [m/s] +const double kDefaultMaxForce = 20; // [N] +} + +namespace flexiv_gripper { + +/** + * @brief Check if the result of the asynchronous command is ready. + * @tparam T The return type of the future. + * @param[in] result_future The future object to check. + * @param[in] timeout Future wait timeout. + * @return True if the function has finished, false otherwise. + */ +template +bool IsResultReady(std::future& result_future, std::chrono::nanoseconds timeout) +{ + return result_future.wait_for(timeout) == std::future_status::ready; +} + +class GripperActionServer : public rclcpp::Node +{ + +public: + using Grasp = flexiv_msgs::action::Grasp; + using GoalHandleGrasp = rclcpp_action::ServerGoalHandle; + + using Move = flexiv_msgs::action::Move; + using GoalHandleMove = rclcpp_action::ServerGoalHandle; + + using GripperCommand = control_msgs::action::GripperCommand; + using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle; + + using Trigger = std_srvs::srv::Trigger; + + explicit GripperActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + // gripper actions + enum class GripperAction + { + kGrasp, + kMove, + kGripperCommand + }; + + // return string of the gripper action + static std::string GetGripperActionName(GripperAction action) + { + switch (action) { + case GripperAction::kGrasp: + return {"Grasping"}; + case GripperAction::kMove: + return {"Moving"}; + case GripperAction::kGripperCommand: + return {"GripperCommand"}; + default: + throw std::invalid_argument("Invalid gripper action"); + } + }; + + // Flexiv RDK + std::unique_ptr robot_; + std::unique_ptr gripper_; + + rclcpp_action::Server::SharedPtr grasp_action_server_; + rclcpp_action::Server::SharedPtr move_action_server_; + rclcpp_action::Server::SharedPtr gripper_command_action_server_; + rclcpp::Service::SharedPtr stop_service_; + rclcpp::TimerBase::SharedPtr state_publish_timer_; + + std::mutex gripper_states_mutex_; + flexiv::rdk::GripperStates current_gripper_states_; + std::atomic is_gripper_moving_ = {false}; + + double default_velocity_; + double default_max_force_; + std::chrono::nanoseconds future_wait_timeout_ {0}; + + // Gripper joint states publisher + rclcpp::Publisher::SharedPtr gripper_joint_states_publisher_; + std::vector gripper_joint_names_; + + /** + * @brief Publish the current gripper states. + */ + void PublishGripperStates(); + + /** + * @brief Stop the gripper. + * @param[out] response Success or failure of the service call. + */ + void StopServiceCallback(const std::shared_ptr& response); + + /** + * @brief Handle the gripper action cancel request. + * @param[in] action Gripper action to cancel. + */ + rclcpp_action::CancelResponse HandleCancel(GripperAction action); + + /** + * @brief Handle the gripper action goal request. + * @param[in] action Gripper action to handle. + */ + rclcpp_action::GoalResponse HandleGoal(GripperAction action); + + /** + * @brief Perform the gripper move action. + */ + void ExecuteMove(const std::shared_ptr& goal_handle); + + /** + * @brief Perform the gripper grasp action. + */ + void ExecuteGrasp(const std::shared_ptr& goal_handle); + + /** + * @brief Perform the gripper command action. + * @param[in] goal_handle The goal handle of the action. + */ + void ExecuteGripperCommand(const std::shared_ptr& goal_handle); + + /** + * @brief Execute the gripper command and return the result. + * @param[in] goal_handle The goal handle of the action. + * @param[in] command The RDK function to execute the gripper command. + */ + void ExecuteGripperCommandHelper(const std::shared_ptr& goal_handle, + const std::function& command); + + /** + * @brief Execute the gripper command and return the result. + * @tparam T Gripper action message type (Grasp or Move). + * @param[in] goal_handle The goal handle of the action. + * @param[in] action The gripper action to execute. + * @param[in] command The RDK function to execute the gripper command. + */ + template + void ExecuteCommand(const std::shared_ptr>& goal_handle, + GripperAction action, const std::function& command) + { + const auto action_name = GetGripperActionName(action); + RCLCPP_INFO(this->get_logger(), "Gripper %s action has been received", action_name.c_str()); + + auto command_execution_result = CommandExecutionResult(command); + + std::future> result_future + = std::async(std::launch::async, command_execution_result); + + while (!IsResultReady(result_future, future_wait_timeout_) && rclcpp::ok()) { + if (goal_handle->is_canceling()) { + gripper_->Stop(); + auto result = result_future.get(); + RCLCPP_INFO( + this->get_logger(), "Gripper %s action has been canceled", action_name.c_str()); + goal_handle->canceled(result); + return; + } + PublishGripperStatesFeedback(goal_handle); + } + + if (rclcpp::ok()) { + const auto result = result_future.get(); + if (result->success) { + RCLCPP_INFO(this->get_logger(), "Gripper %s action has been completed", + action_name.c_str()); + goal_handle->succeed(result); + } else { + RCLCPP_ERROR( + this->get_logger(), "Gripper %s action has failed", action_name.c_str()); + goal_handle->abort(result); + } + } + } + + /** + * @brief Return the command execution result. + * @tparam T Gripper action message type (Grasp or Move). + * @param[in] command The function to execute the gripper command. + * @return Success or failure of the command execution. + */ + template + std::function()> CommandExecutionResult( + const std::function& command) + { + return std::function()>([command]() { + auto result = std::make_shared(); + try { + command(); + result->success = true; + } catch (const std::exception& e) { + result->success = false; + result->error = e.what(); + } + return result; + }); + } + + /** + * @brief Publish the gripper states feedback in the action server. + * @tparam T Gripper action message type (Grasp or Move). + * @param[in] goal_handle The goal handle of the action. + */ + template + void PublishGripperStatesFeedback( + const std::shared_ptr>& goal_handle) + { + auto feedback = std::make_shared(); + std::lock_guard lock(gripper_states_mutex_); + feedback->current_width = current_gripper_states_.width; + feedback->current_force = current_gripper_states_.force; + feedback->moving = is_gripper_moving_; + goal_handle->publish_feedback(feedback); + } + + /** + * @brief Publish the gripper command feedback in the action server. + * @param[in] goal_handle The goal handle of the action. + */ + void PublishGripperCommandFeedback( + const std::shared_ptr>& goal_handle); +}; + +} // namespace flexiv_gripper + +#endif /* FLEXIV_GRIPPER__GRIPPER_ACTION_SERVER_HPP_ */ diff --git a/flexiv_gripper/launch/flexiv_gripper.launch.py b/flexiv_gripper/launch/flexiv_gripper.launch.py new file mode 100644 index 0000000..b80d789 --- /dev/null +++ b/flexiv_gripper/launch/flexiv_gripper.launch.py @@ -0,0 +1,66 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_sn_param_name = "robot_sn" + use_fake_hardware_param_name = "use_fake_hardware" + gripper_joint_names_param_name = "gripper_joint_names" + + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + robot_sn_param_name, + description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + use_fake_hardware_param_name, + default_value="false", + description="Start gripper with fake gripper joint states.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + gripper_joint_names_param_name, + description="Control joint names of the mounted gripper.", + default_value="[finger_width_joint]", + ) + ) + + # Initialize arguments + robot_sn = LaunchConfiguration(robot_sn_param_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name) + gripper_joint_names = LaunchConfiguration(gripper_joint_names_param_name) + + gripper_config_file = PathJoinSubstitution( + [FindPackageShare("flexiv_gripper"), "config", "flexiv_gripper_node.yaml"] + ) + + # Flexiv gripper node + flexiv_gripper_node = Node( + package="flexiv_gripper", + executable="flexiv_gripper_node", + name="flexiv_gripper_node", + parameters=[ + {"robot_sn": robot_sn, "gripper_joint_names": gripper_joint_names}, + gripper_config_file, + ], + condition=UnlessCondition(use_fake_hardware), + ) + + nodes = [flexiv_gripper_node] + + return LaunchDescription(declared_arguments + nodes) diff --git a/flexiv_gripper/package.xml b/flexiv_gripper/package.xml new file mode 100644 index 0000000..1b6f462 --- /dev/null +++ b/flexiv_gripper/package.xml @@ -0,0 +1,28 @@ + + + + flexiv_gripper + 1.5.0 + Package implementing the action server of Flexiv gripper control + Mun Seng Phoon + Apache License 2.0 + Mun Seng Phoon + + ament_cmake + + control_msgs + flexiv_hardware + flexiv_msgs + rclcpp + rclcpp_action + rclcpp_components + sensor_msgs + std_srvs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/flexiv_gripper/src/gripper_action_server.cpp b/flexiv_gripper/src/gripper_action_server.cpp new file mode 100644 index 0000000..4c1ac49 --- /dev/null +++ b/flexiv_gripper/src/gripper_action_server.cpp @@ -0,0 +1,297 @@ +#include + +#include "rclcpp_components/register_node_macro.hpp" + +#include "flexiv_gripper/gripper_action_server.hpp" + +namespace flexiv_gripper { + +GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options) +: Node("flexiv_gripper_node", options) +{ + this->declare_parameter("robot_sn", std::string()); + this->declare_parameter("state_publish_rate", kDefaultStatePublishRate); + this->declare_parameter("feedback_publish_rate", kDefaultFeedbackPublishRate); + this->declare_parameter("default_velocity", kDefaultVelocity); + this->declare_parameter("default_max_force", kDefaultMaxForce); + this->declare_parameter("gripper_joint_names", std::vector()); + + std::string robot_sn; + if (!this->get_parameter("robot_sn", robot_sn)) { + RCLCPP_FATAL(this->get_logger(), "Parameter 'robot_sn' is not set"); + throw std::invalid_argument("Parameter 'robot_sn' is not set"); + } + + this->default_velocity_ = this->get_parameter("default_velocity").as_double(); + this->default_max_force_ = this->get_parameter("default_max_force").as_double(); + + if (!this->get_parameter("gripper_joint_names", this->gripper_joint_names_)) { + RCLCPP_WARN(this->get_logger(), "Parameter 'gripper_joint_names' is not set"); + this->gripper_joint_names_ = {""}; + } + + const double kStatePublishRate + = static_cast(this->get_parameter("state_publish_rate").as_int()); + const double kFeedbackPublishRate + = static_cast(this->get_parameter("feedback_publish_rate").as_int()); + this->future_wait_timeout_ = rclcpp::WallRate(kFeedbackPublishRate).period(); + + try { + RCLCPP_INFO(this->get_logger(), "Connecting to robot %s ...", robot_sn.c_str()); + robot_ = std::make_unique(robot_sn); + + RCLCPP_INFO(this->get_logger(), "Successfully connected to robot"); + + // Clear fault on robot server if any + if (robot_->fault()) { + RCLCPP_WARN(this->get_logger(), "Fault occurred on robot server, trying to clear ..."); + // Try to clear the fault + robot_->ClearFault(); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Check again + if (robot_->fault()) { + RCLCPP_FATAL(get_logger(), "Fault cannot be cleared, exiting ..."); + throw std::runtime_error("Fault cannot be cleared"); + } + RCLCPP_INFO(this->get_logger(), "Fault on robot server is cleared"); + } + + // Enable the robot + if (!robot_->operational(false)) { + RCLCPP_INFO(this->get_logger(), "Enabling robot ..."); + robot_->Enable(); + + // Wait for the robot to become operational + while (!robot_->operational(false)) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + RCLCPP_INFO(this->get_logger(), "Robot is now operational"); + } + + // Gripper control is not available if the robot is in IDLE mode, so switch to some mode + // other than IDLE, e.g. NRT_JOINT_POSITION + if (robot_->mode() == flexiv::rdk::Mode::IDLE) { + robot_->SwitchMode(flexiv::rdk::Mode::NRT_JOINT_POSITION); + } + + RCLCPP_INFO(this->get_logger(), "Initializing Flexiv gripper control interface"); + this->gripper_ = std::make_unique(*robot_); + + // Manually initialize the gripper, not all grippers need this step + RCLCPP_INFO( + this->get_logger(), "Initializing gripper, this process takes about 10 seconds .."); + gripper_->Init(); + RCLCPP_INFO(this->get_logger(), "Gripper initialization completed"); + + // Get the current gripper states + this->current_gripper_states_ = gripper_->states(); + this->is_gripper_moving_ = gripper_->moving(); + } catch (const std::exception& e) { + RCLCPP_FATAL(this->get_logger(), "%s", e.what()); + throw e; + } + + // Create the stop service server + this->stop_service_ + = create_service("~/stop", [this](std::shared_ptr /*request*/, + std::shared_ptr response) { + return StopServiceCallback(std::move(response)); + }); + + // Create the action servers + const auto kMoveAction = GripperAction::kMove; + this->move_action_server_ = rclcpp_action::create_server( + this, "~/move", + [this, kMoveAction](auto /*uuid*/, auto /*goal*/) { return HandleGoal(kMoveAction); }, + [this, kMoveAction](const auto& /*goal_handle*/) { return HandleCancel(kMoveAction); }, + [this](const auto& goal_handle) { + return std::thread {[this, goal_handle]() { ExecuteMove(goal_handle); }}.detach(); + }); + + const auto kGraspAction = GripperAction::kGrasp; + this->grasp_action_server_ = rclcpp_action::create_server( + this, "~/grasp", + [this, kGraspAction](auto /*uuid*/, auto /*goal*/) { return HandleGoal(kGraspAction); }, + [this, kGraspAction](const auto& /*goal_handle*/) { return HandleCancel(kGraspAction); }, + [this](const auto& goal_handle) { + return std::thread {[this, goal_handle]() { ExecuteGrasp(goal_handle); }}.detach(); + }); + + const auto kGripperCommandAction = GripperAction::kGripperCommand; + this->gripper_command_action_server_ = rclcpp_action::create_server( + this, "~/gripper_action", + [this, kGripperCommandAction]( + auto /*uuid*/, auto /*goal*/) { return HandleGoal(kGripperCommandAction); }, + [this, kGripperCommandAction]( + const auto& /*goal_handle*/) { return HandleCancel(kGripperCommandAction); }, + [this](const auto& goal_handle) { + return std::thread {[this, goal_handle]() { + ExecuteGripperCommand(goal_handle); + }}.detach(); + }); + + this->gripper_joint_states_publisher_ + = this->create_publisher("~/gripper_joint_states", 1); + this->state_publish_timer_ = this->create_wall_timer( + rclcpp::WallRate(kStatePublishRate).period(), [this]() { return PublishGripperStates(); }); +} + +rclcpp_action::CancelResponse GripperActionServer::HandleCancel(GripperAction action) +{ + const auto action_name = GetGripperActionName(action); + RCLCPP_INFO(this->get_logger(), "Canceling %s action", action_name.c_str()); + return rclcpp_action::CancelResponse::ACCEPT; +} + +rclcpp_action::GoalResponse GripperActionServer::HandleGoal(GripperAction action) +{ + const auto action_name = GetGripperActionName(action); + RCLCPP_INFO(this->get_logger(), "Received %s action request", action_name.c_str()); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void GripperActionServer::ExecuteMove(const std::shared_ptr& goal_handle) +{ + auto command = [this, goal_handle]() { + const auto goal = goal_handle->get_goal(); + gripper_->Move(goal->width, goal->velocity, goal->max_force); + }; + ExecuteCommand(goal_handle, GripperAction::kMove, command); +} + +void GripperActionServer::ExecuteGrasp(const std::shared_ptr& goal_handle) +{ + auto command = [this, goal_handle]() { + const auto goal = goal_handle->get_goal(); + gripper_->Grasp(goal->force); + }; + ExecuteCommand(goal_handle, GripperAction::kGrasp, command); +} + +void GripperActionServer::ExecuteGripperCommand( + const std::shared_ptr& goal_handle) +{ + const auto goal = goal_handle->get_goal(); + const double target_width = goal->command.position; + + std::unique_lock guard(gripper_states_mutex_); + auto result = std::make_shared(); + const double current_width = current_gripper_states_.width; + if (target_width > current_gripper_states_.max_width || target_width < 0) { + RCLCPP_ERROR(this->get_logger(), "Invalid gripper target width: %f. Max width = %f", + target_width, current_gripper_states_.max_width); + goal_handle->abort(result); + return; + } + if (std::abs(target_width - current_width) < 1e-3) { + RCLCPP_INFO(this->get_logger(), "Gripper is already at the target width: %f", target_width); + result->effort = current_gripper_states_.force; + result->position = current_gripper_states_.width; + result->reached_goal = true; + result->stalled = false; + goal_handle->succeed(result); + return; + } + guard.unlock(); + + auto command = [this, target_width]() { + gripper_->Move(target_width, kDefaultVelocity, kDefaultMaxForce); + }; + + ExecuteGripperCommandHelper(goal_handle, command); +} + +void GripperActionServer::ExecuteGripperCommandHelper( + const std::shared_ptr& goal_handle, + const std::function& command) +{ + const auto action_name = GetGripperActionName(GripperAction::kGripperCommand); + RCLCPP_INFO(this->get_logger(), "Gripper %s action has been received", action_name.c_str()); + + auto command_execution_result = [this, command]() { + auto result = std::make_shared(); + try { + command(); + result->reached_goal = true; + } catch (const std::exception& e) { + result->reached_goal = false; + RCLCPP_INFO(this->get_logger(), "Gripper command failed: %s", e.what()); + } + return result; + }; + + std::future> result_future + = std::async(std::launch::async, command_execution_result); + + while (!IsResultReady(result_future, future_wait_timeout_) && rclcpp::ok()) { + if (goal_handle->is_canceling()) { + gripper_->Stop(); + auto result = result_future.get(); + RCLCPP_INFO( + this->get_logger(), "Gripper %s action has been canceled", action_name.c_str()); + goal_handle->canceled(result); + return; + } + PublishGripperCommandFeedback(goal_handle); + } + + if (rclcpp::ok()) { + const auto result = result_future.get(); + std::lock_guard guard(gripper_states_mutex_); + result->position = current_gripper_states_.width; + result->effort = current_gripper_states_.force; + if (result->reached_goal) { + RCLCPP_INFO( + this->get_logger(), "Gripper %s action has been completed", action_name.c_str()); + goal_handle->succeed(result); + } else { + RCLCPP_ERROR(this->get_logger(), "Gripper %s action has failed", action_name.c_str()); + goal_handle->abort(result); + } + } +} + +void GripperActionServer::StopServiceCallback(const std::shared_ptr& response) +{ + RCLCPP_INFO(this->get_logger(), "Stopping the gripper..."); + auto result = CommandExecutionResult([this]() { gripper_->Stop(); })(); + response->success = result->success; + response->message = result->error; + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Gripper has been stopped"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to stop the gripper"); + } + if (!response->message.empty()) { + RCLCPP_ERROR(this->get_logger(), "Error message: %s", response->message.c_str()); + } +} + +void GripperActionServer::PublishGripperStates() +{ + std::lock_guard lock(gripper_states_mutex_); + this->current_gripper_states_ = gripper_->states(); + // Modify the gripper joint states based on the mounted gripper type + // The gripper joint states below is for the Flexiv Grav GN-01 gripper + sensor_msgs::msg::JointState gripper_joint_states; + gripper_joint_states.header.stamp = this->now(); + gripper_joint_states.name.push_back(this->gripper_joint_names_[0]); + gripper_joint_states.position.push_back(this->current_gripper_states_.width); + gripper_joint_states.velocity.push_back(0.0); + gripper_joint_states.effort.push_back(this->current_gripper_states_.force); + this->gripper_joint_states_publisher_->publish(gripper_joint_states); +} + +void GripperActionServer::PublishGripperCommandFeedback( + const std::shared_ptr>& goal_handle) +{ + auto feedback = std::make_shared(); + std::lock_guard guard(gripper_states_mutex_); + feedback->position = current_gripper_states_.width; + feedback->effort = current_gripper_states_.force; + goal_handle->publish_feedback(feedback); +} + +} // namespace flexiv_gripper + +RCLCPP_COMPONENTS_REGISTER_NODE(flexiv_gripper::GripperActionServer) diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp index 842cef0..ae16863 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp @@ -107,6 +107,10 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface std::vector hw_commands_gpio_out_; std::vector hw_states_gpio_in_; + // Current digital output ports + std::vector current_ports_indices_; + std::vector current_ports_values_; + static rclcpp::Logger getLogger(); // control modes diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml index ecce1e2..6477d17 100644 --- a/flexiv_hardware/package.xml +++ b/flexiv_hardware/package.xml @@ -2,7 +2,7 @@ flexiv_hardware - 1.4.0 + 1.5.0 Hardware interfaces to Flexiv robots for ROS 2 control Mun Seng Phoon Apache 2.0 diff --git a/flexiv_hardware/rdk b/flexiv_hardware/rdk index 7f020e6..9ee458e 160000 --- a/flexiv_hardware/rdk +++ b/flexiv_hardware/rdk @@ -1 +1 @@ -Subproject commit 7f020e6bf37bd09e534cc5da591183af3ccec4c9 +Subproject commit 9ee458ecba3c0b0cc0bcad0dc2d6f1327846c53f diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index d1d0125..3f5928c 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -17,6 +17,13 @@ #include "flexiv/rdk/robot.hpp" #include "flexiv_hardware/flexiv_hardware_interface.hpp" +namespace { + +constexpr double kMaxJointVelocity = 2.0; +constexpr double kMaxJointAcceleration = 3.0; + +} + namespace flexiv_hardware { hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( @@ -257,7 +264,7 @@ hardware_interface::return_type FlexivHardwareInterface::read( hw_states_joint_efforts_ = robot_->states().tau; // Read GPIO input states - auto gpio_in = robot_->ReadDigitalInput(); + auto gpio_in = robot_->digital_inputs(); for (size_t i = 0; i < hw_states_gpio_in_.size(); i++) { hw_states_gpio_in_[i] = static_cast(gpio_in[i]); } @@ -274,6 +281,9 @@ hardware_interface::return_type FlexivHardwareInterface::write( std::vector target_vel(robot_->info().DoF); std::vector target_acc(robot_->info().DoF); + std::vector max_vel(robot_->info().DoF, kMaxJointVelocity); + std::vector max_acc(robot_->info().DoF, kMaxJointAcceleration); + bool isNanPos = false; bool isNanVel = false; bool isNanEff = false; @@ -289,15 +299,15 @@ hardware_interface::return_type FlexivHardwareInterface::write( } } - if (position_controller_running_ && robot_->mode() == flexiv::rdk::Mode::RT_JOINT_POSITION + if (position_controller_running_ && robot_->mode() == flexiv::rdk::Mode::NRT_JOINT_POSITION && !isNanPos) { target_pos = hw_commands_joint_positions_; - robot_->StreamJointPosition(target_pos, target_vel, target_acc); + robot_->SendJointPosition(target_pos, target_vel, target_acc, max_vel, max_acc); } else if (velocity_controller_running_ - && robot_->mode() == flexiv::rdk::Mode::RT_JOINT_POSITION && !isNanVel) { + && robot_->mode() == flexiv::rdk::Mode::NRT_JOINT_POSITION && !isNanVel) { target_pos = hw_commands_joint_positions_; target_vel = hw_commands_joint_velocities_; - robot_->StreamJointPosition(target_pos, target_vel, target_acc); + robot_->SendJointPosition(target_pos, target_vel, target_acc, max_vel, max_acc); } else if (torque_controller_running_ && robot_->mode() == flexiv::rdk::Mode::RT_JOINT_TORQUE && !isNanEff) { std::vector target_torque(robot_->info().DoF); @@ -315,8 +325,18 @@ hardware_interface::return_type FlexivHardwareInterface::write( ports_indices.push_back(i); ports_values.push_back(static_cast(hw_commands_gpio_out_[i])); } + // Check if there are changes in the digital output values + bool digital_outputs_changed = false; + if (current_ports_indices_ != ports_indices || current_ports_values_ != ports_values) { + digital_outputs_changed = true; + } + current_ports_indices_ = ports_indices; + current_ports_values_ = ports_values; - robot_->WriteDigitalOutput(ports_indices, ports_values); + // Set digital outputs + if (!ports_indices.empty() && !ports_values.empty() && digital_outputs_changed) { + robot_->SetDigitalOutputs(ports_indices, ports_values); + } return hardware_interface::return_type::OK; } @@ -410,7 +430,7 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw std::numeric_limits::quiet_NaN()); // Set to joint position mode - robot_->SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION); + robot_->SwitchMode(flexiv::rdk::Mode::NRT_JOINT_POSITION); position_controller_running_ = true; } else if (start_modes_.size() != 0 @@ -425,7 +445,7 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw std::numeric_limits::quiet_NaN()); // Set to joint position mode - robot_->SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION); + robot_->SwitchMode(flexiv::rdk::Mode::NRT_JOINT_POSITION); velocity_controller_running_ = true; } else if (start_modes_.size() != 0 diff --git a/flexiv_moveit_config/config/joint_limits.yaml b/flexiv_moveit_config/config/joint_limits.yaml index 1617955..f6ca334 100644 --- a/flexiv_moveit_config/config/joint_limits.yaml +++ b/flexiv_moveit_config/config/joint_limits.yaml @@ -10,7 +10,7 @@ default_acceleration_scaling_factor: 0.1 joint_limits: joint1: has_velocity_limits: true - max_velocity: 2.0944 # Rizon 4 + max_velocity: 2.0944 # Rizon 4 # max_velocity: 1.7453 # Rizon 10 has_acceleration_limits: false max_acceleration: 0 @@ -50,3 +50,8 @@ joint_limits: # max_velocity: 3.8397 # Rizon 10 has_acceleration_limits: false max_acceleration: 0 + finger_width_joint: + has_velocity_limits: true + max_velocity: 0.5 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/flexiv_moveit_config/config/moveit_controllers.yaml b/flexiv_moveit_config/config/moveit_controllers.yaml index 2b213ff..8866277 100644 --- a/flexiv_moveit_config/config/moveit_controllers.yaml +++ b/flexiv_moveit_config/config/moveit_controllers.yaml @@ -1,5 +1,6 @@ controller_names: - rizon_arm_controller + - flexiv_gripper_node rizon_arm_controller: action_ns: follow_joint_trajectory @@ -13,3 +14,10 @@ rizon_arm_controller: - joint5 - joint6 - joint7 + +flexiv_gripper_node: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - finger_width_joint diff --git a/flexiv_moveit_config/config/ompl_planning.yaml b/flexiv_moveit_config/config/ompl_planning.yaml index 3e76a31..69cf12f 100644 --- a/flexiv_moveit_config/config/ompl_planning.yaml +++ b/flexiv_moveit_config/config/ompl_planning.yaml @@ -1,157 +1,176 @@ planner_configs: - AnytimePathShortening: - type: geometric::AnytimePathShortening - shortcut: true # Attempt to shortcut all new solution paths - hybridize: true # Compute hybrid solution trajectories - max_hybrid_paths: 24 # Number of hybrid paths generated per iteration - num_planners: 4 # The number of default planners to use for planning - planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" - SBL: + SBLkConfigDefault: type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - EST: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECE: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECE: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECE: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRT: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnect: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstar: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRT: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() - PRM: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstar: + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: type: geometric::PRMstar - FMT: + FMTkConfigDefault: type: geometric::FMT - num_samples: 1000 # number of states that the planner should sample. default: 1000 - radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 - nearest_k: 1 # use Knearest strategy. default: 1 - cache_cc: 1 # use collision checking cache. default: 1 - heuristics: 0 # activate cost to go heuristics. default: 0 - extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - BFMT: + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: type: geometric::BFMT - num_samples: 1000 # number of states that the planner should sample. default: 1000 - radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 - nearest_k: 1 # use the Knearest strategy. default: 1 - balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 - optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 - heuristics: 1 # activates cost to go heuristics. default: 1 - cache_cc: 1 # use the collision checking cache. default: 1 - extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - PDST: + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: type: geometric::PDST - STRIDE: + STRIDEkConfigDefault: type: geometric::STRIDE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 - degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 - max_degree: 18 # max degree of a node in the GNAT. default: 12 - min_degree: 12 # min degree of a node in the GNAT. default: 12 - max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 - estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 - min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 - BiTRRT: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: type: geometric::BiTRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 - init_temperature: 100 # initial temperature. default: 100 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf - LBTRRT: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: type: geometric::LBTRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - epsilon: 0.4 # optimality approximation factor. default: 0.4 - BiEST: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: type: geometric::BiEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ProjEST: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: type: geometric::ProjEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LazyPRM: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: type: geometric::LazyPRM - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - LazyPRMstar: + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: type: geometric::LazyPRMstar - SPARS: + SPARSkConfigDefault: type: geometric::SPARS - stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 - sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 - dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 - max_failures: 1000 # maximum consecutive failure limit. default: 1000 - SPARStwo: + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: type: geometric::SPARStwo - stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 - sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 - dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 - max_failures: 5000 # maximum consecutive failure limit. default: 5000 -flexiv_arm: + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt +rizon_arm: planner_configs: - - AnytimePathShortening - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo - projection_evaluator: joints(joint1,joint2) - longest_valid_segment_fraction: 0.005 + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +grav_gripper: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault diff --git a/flexiv_moveit_config/package.xml b/flexiv_moveit_config/package.xml index 26e7eed..af4083b 100644 --- a/flexiv_moveit_config/package.xml +++ b/flexiv_moveit_config/package.xml @@ -2,7 +2,7 @@ flexiv_moveit_config - 1.4.0 + 1.5.0 MoveIt2 configuration and launch files for Flexiv Rizon robots Mun Seng Phoon Apache 2.0 diff --git a/flexiv_moveit_config/srdf/grav.srdf.xacro b/flexiv_moveit_config/srdf/grav.srdf.xacro new file mode 100644 index 0000000..8890b7c --- /dev/null +++ b/flexiv_moveit_config/srdf/grav.srdf.xacro @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flexiv_moveit_config/srdf/rizon.srdf.xacro b/flexiv_moveit_config/srdf/rizon.srdf.xacro index 40377db..43a6f99 100644 --- a/flexiv_moveit_config/srdf/rizon.srdf.xacro +++ b/flexiv_moveit_config/srdf/rizon.srdf.xacro @@ -1,13 +1,21 @@ - + - + + - + - + + + + + + + + diff --git a/flexiv_msgs/CMakeLists.txt b/flexiv_msgs/CMakeLists.txt index 58022c6..8e0206b 100644 --- a/flexiv_msgs/CMakeLists.txt +++ b/flexiv_msgs/CMakeLists.txt @@ -18,14 +18,15 @@ set(msg_files # set(srv_files # ) -# set(action_files -# action/SetMode.action -# ) +set(action_files + action/Grasp.action + action/Move.action +) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} # ${srv_files} - # ${action_files} + ${action_files} DEPENDENCIES std_msgs geometry_msgs diff --git a/flexiv_msgs/action/Grasp.action b/flexiv_msgs/action/Grasp.action new file mode 100644 index 0000000..cef5889 --- /dev/null +++ b/flexiv_msgs/action/Grasp.action @@ -0,0 +1,8 @@ +float64 force # N +--- +bool success +string error +--- +float64 current_width # m +float64 current_force # N +bool moving diff --git a/flexiv_msgs/action/Move.action b/flexiv_msgs/action/Move.action new file mode 100644 index 0000000..5523983 --- /dev/null +++ b/flexiv_msgs/action/Move.action @@ -0,0 +1,10 @@ +float64 width # m +float64 velocity # m/s +float64 max_force # N +--- +bool success +string error +--- +float64 current_width # m +float64 current_force # N +bool moving diff --git a/flexiv_msgs/package.xml b/flexiv_msgs/package.xml index 7f9a4e7..303ede2 100644 --- a/flexiv_msgs/package.xml +++ b/flexiv_msgs/package.xml @@ -2,7 +2,7 @@ flexiv_msgs - 1.4.0 + 1.5.0 Robot states messages definiton used in RDK Mun Seng Phoon Apache License 2.0 diff --git a/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py b/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py index 1af64c7..0afeb2d 100644 --- a/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py +++ b/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py @@ -18,17 +18,22 @@ def __init__(self): super().__init__("sine_sweep_impedance_controller") # Declare all parameters self.declare_parameter("controller_name", "joint_impedance_controller") + self.declare_parameter("joints", [""]) self.declare_parameter("wait_sec_between_publish", 0.001) self.declare_parameter("speed_scaling", 1.0) # Read parameters controller_name = self.get_parameter("controller_name").value + self.joints = self.get_parameter("joints").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value self.speed_scaling = self.get_parameter("speed_scaling").value + if self.joints is None or len(self.joints) == 0: + raise Exception('"joints" parameter is not set!') + publish_topic = "/" + controller_name + "/" + "commands" - self.init_pos = [0.0] * 7 + self.init_pos = [0.0] * len(self.joints) self.publisher_ = self.create_publisher(JointPosVel, publish_topic, 1) self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) @@ -42,7 +47,7 @@ def __init__(self): def timer_callback(self): if self.joint_state_msg_received: target_pos = self.init_pos.copy() - for i in range(7): + for i in range(len(self.joints)): target_pos[i] = self.init_pos[i] + SWING_AMP * math.sin( 2 * math.pi * SWING_FREQ * self.speed_scaling * self.loop_time ) @@ -53,11 +58,14 @@ def timer_callback(self): def joint_state_callback(self, msg): if not self.joint_state_msg_received: - # retrieve joint states in correct order - joint_order = [2, 5, 6, 1, 4, 7, 3] - positions = msg.position - # set the init pos in correct order - self.init_pos = [i for _, i in sorted(zip(joint_order, positions))] + # retrieve joint states by the order of the joint names + for name in self.joints: + if name not in msg.name: + raise Exception(f"Joint {name} not found in joint_states!") + joint_msg_index = msg.name.index(name) + joint_position = msg.position[joint_msg_index] + index = self.joints.index(name) + self.init_pos[index] = joint_position self.joint_state_msg_received = True else: return diff --git a/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_position_controller.py b/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_position_controller.py index e0a8222..d9286aa 100644 --- a/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_position_controller.py +++ b/flexiv_test_nodes/flexiv_test_nodes/sine_sweep_position_controller.py @@ -17,17 +17,22 @@ def __init__(self): super().__init__("sine_sweep_position_controller") # Declare all parameters self.declare_parameter("controller_name", "forward_position_controller") + self.declare_parameter("joints", [""]) self.declare_parameter("wait_sec_between_publish", 0.001) self.declare_parameter("speed_scaling", 1.0) # Read parameters controller_name = self.get_parameter("controller_name").value + self.joints = self.get_parameter("joints").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value self.speed_scaling = self.get_parameter("speed_scaling").value + if self.joints is None or len(self.joints) == 0: + raise Exception('"joints" parameter is not set!') + publish_topic = "/" + controller_name + "/" + "commands" - self.init_pos = [0.0] * 7 + self.init_pos = [0.0] * len(self.joints) self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) @@ -41,7 +46,7 @@ def __init__(self): def timer_callback(self): if self.joint_state_msg_received: target_pos = self.init_pos.copy() - for i in range(7): + for i in range(len(self.joints)): target_pos[i] = self.init_pos[i] + SWING_AMP * math.sin( 2 * math.pi * SWING_FREQ * self.speed_scaling * self.loop_time ) @@ -52,11 +57,14 @@ def timer_callback(self): def joint_state_callback(self, msg): if not self.joint_state_msg_received: - # retrieve joint states in correct order - joint_order = [2, 5, 6, 1, 4, 7, 3] - positions = msg.position - # set the init pos in correct order - self.init_pos = [i for _, i in sorted(zip(joint_order, positions))] + # retrieve joint states by the order of the joint names + for name in self.joints: + if name not in msg.name: + raise Exception(f"Joint {name} not found in joint_states!") + joint_msg_index = msg.name.index(name) + joint_position = msg.position[joint_msg_index] + index = self.joints.index(name) + self.init_pos[index] = joint_position self.joint_state_msg_received = True else: return diff --git a/flexiv_test_nodes/package.xml b/flexiv_test_nodes/package.xml index e24ef98..005c185 100644 --- a/flexiv_test_nodes/package.xml +++ b/flexiv_test_nodes/package.xml @@ -2,7 +2,7 @@ flexiv_test_nodes - 1.4.0 + 1.5.0 Demo nodes for testing flexiv_ros2 Mun Seng Phoon Apache-2.0