From c92812ed66610f2e742237f705a33215d954559c Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 8 Mar 2021 16:01:55 -0600 Subject: [PATCH 01/85] Actually added the Arm MoveIt files this time. --- src/wroboarm_21/.setup_assistant | 11 + src/wroboarm_21/CMakeLists.txt | 10 + src/wroboarm_21/config/chomp_planning.yaml | 18 + src/wroboarm_21/config/fake_controllers.yaml | 12 + src/wroboarm_21/config/joint_limits.yaml | 40 + .../config/joint_names_wroboarm_21.yaml | 1 + src/wroboarm_21/config/kinematics.yaml | 4 + src/wroboarm_21/config/ompl_planning.yaml | 156 ++++ src/wroboarm_21/config/ros_controllers.yaml | 35 + src/wroboarm_21/config/sensors_3d.yaml | 3 + src/wroboarm_21/config/wroboarm_21.srdf | 37 + src/wroboarm_21/export.log | 365 ++++++++++ .../launch/chomp_planning_pipeline.launch.xml | 21 + .../launch/default_warehouse_db.launch | 15 + src/wroboarm_21/launch/demo.launch | 64 ++ src/wroboarm_21/launch/demo_gazebo.launch | 74 ++ src/wroboarm_21/launch/display.launch | 20 + .../fake_moveit_controller_manager.launch.xml | 9 + src/wroboarm_21/launch/gazebo.launch | 23 + .../launch/joystick_control.launch | 17 + src/wroboarm_21/launch/move_group.launch | 82 +++ src/wroboarm_21/launch/moveit.rviz | 689 ++++++++++++++++++ src/wroboarm_21/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 25 + .../launch/planning_context.launch | 25 + .../launch/planning_pipeline.launch.xml | 10 + src/wroboarm_21/launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 21 + .../launch/sensor_manager.launch.xml | 17 + src/wroboarm_21/launch/setup_assistant.launch | 15 + .../launch/trajectory_execution.launch.xml | 20 + src/wroboarm_21/launch/warehouse.launch | 15 + .../launch/warehouse_settings.launch.xml | 16 + ...rm_21_moveit_controller_manager.launch.xml | 10 + ...oboarm_21_moveit_sensor_manager.launch.xml | 3 + src/wroboarm_21/meshes/elbow.STL | Bin 0 -> 8084 bytes src/wroboarm_21/meshes/forearm.STL | Bin 0 -> 14084 bytes src/wroboarm_21/meshes/gearbox.STL | Bin 0 -> 12884 bytes src/wroboarm_21/meshes/manipulator.STL | Bin 0 -> 4884 bytes src/wroboarm_21/meshes/turntable.STL | Bin 0 -> 4484 bytes src/wroboarm_21/meshes/upperarm.STL | Bin 0 -> 13484 bytes src/wroboarm_21/meshes/wrist.STL | Bin 0 -> 21884 bytes src/wroboarm_21/package.xml | 35 + src/wroboarm_21/urdf/wroboarm_21.csv | 8 + src/wroboarm_21/urdf/wroboarm_21.urdf | 365 ++++++++++ 45 files changed, 2317 insertions(+) create mode 100644 src/wroboarm_21/.setup_assistant create mode 100644 src/wroboarm_21/CMakeLists.txt create mode 100644 src/wroboarm_21/config/chomp_planning.yaml create mode 100644 src/wroboarm_21/config/fake_controllers.yaml create mode 100644 src/wroboarm_21/config/joint_limits.yaml create mode 100644 src/wroboarm_21/config/joint_names_wroboarm_21.yaml create mode 100644 src/wroboarm_21/config/kinematics.yaml create mode 100644 src/wroboarm_21/config/ompl_planning.yaml create mode 100644 src/wroboarm_21/config/ros_controllers.yaml create mode 100644 src/wroboarm_21/config/sensors_3d.yaml create mode 100644 src/wroboarm_21/config/wroboarm_21.srdf create mode 100644 src/wroboarm_21/export.log create mode 100644 src/wroboarm_21/launch/chomp_planning_pipeline.launch.xml create mode 100644 src/wroboarm_21/launch/default_warehouse_db.launch create mode 100644 src/wroboarm_21/launch/demo.launch create mode 100644 src/wroboarm_21/launch/demo_gazebo.launch create mode 100644 src/wroboarm_21/launch/display.launch create mode 100644 src/wroboarm_21/launch/fake_moveit_controller_manager.launch.xml create mode 100644 src/wroboarm_21/launch/gazebo.launch create mode 100644 src/wroboarm_21/launch/joystick_control.launch create mode 100644 src/wroboarm_21/launch/move_group.launch create mode 100644 src/wroboarm_21/launch/moveit.rviz create mode 100644 src/wroboarm_21/launch/moveit_rviz.launch create mode 100644 src/wroboarm_21/launch/ompl_planning_pipeline.launch.xml create mode 100644 src/wroboarm_21/launch/planning_context.launch create mode 100644 src/wroboarm_21/launch/planning_pipeline.launch.xml create mode 100644 src/wroboarm_21/launch/ros_controllers.launch create mode 100644 src/wroboarm_21/launch/run_benchmark_ompl.launch create mode 100644 src/wroboarm_21/launch/sensor_manager.launch.xml create mode 100644 src/wroboarm_21/launch/setup_assistant.launch create mode 100644 src/wroboarm_21/launch/trajectory_execution.launch.xml create mode 100644 src/wroboarm_21/launch/warehouse.launch create mode 100644 src/wroboarm_21/launch/warehouse_settings.launch.xml create mode 100644 src/wroboarm_21/launch/wroboarm_21_moveit_controller_manager.launch.xml create mode 100644 src/wroboarm_21/launch/wroboarm_21_moveit_sensor_manager.launch.xml create mode 100644 src/wroboarm_21/meshes/elbow.STL create mode 100644 src/wroboarm_21/meshes/forearm.STL create mode 100644 src/wroboarm_21/meshes/gearbox.STL create mode 100644 src/wroboarm_21/meshes/manipulator.STL create mode 100644 src/wroboarm_21/meshes/turntable.STL create mode 100644 src/wroboarm_21/meshes/upperarm.STL create mode 100644 src/wroboarm_21/meshes/wrist.STL create mode 100644 src/wroboarm_21/package.xml create mode 100644 src/wroboarm_21/urdf/wroboarm_21.csv create mode 100644 src/wroboarm_21/urdf/wroboarm_21.urdf diff --git a/src/wroboarm_21/.setup_assistant b/src/wroboarm_21/.setup_assistant new file mode 100644 index 00000000..478660a7 --- /dev/null +++ b/src/wroboarm_21/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: wroboarm_21 + relative_path: urdf/wroboarm_21.urdf + xacro_args: "" + SRDF: + relative_path: config/wroboarm_21.srdf + CONFIG: + author_name: me + author_email: me@me.me + generated_timestamp: 1614898844 \ No newline at end of file diff --git a/src/wroboarm_21/CMakeLists.txt b/src/wroboarm_21/CMakeLists.txt new file mode 100644 index 00000000..1fbbe71b --- /dev/null +++ b/src/wroboarm_21/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(wroboarm_21) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/wroboarm_21/config/chomp_planning.yaml b/src/wroboarm_21/config/chomp_planning.yaml new file mode 100644 index 00000000..75258e53 --- /dev/null +++ b/src/wroboarm_21/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/src/wroboarm_21/config/fake_controllers.yaml b/src/wroboarm_21/config/fake_controllers.yaml new file mode 100644 index 00000000..c357a98e --- /dev/null +++ b/src/wroboarm_21/config/fake_controllers.yaml @@ -0,0 +1,12 @@ +controller_list: + - name: fake_arm_controller + joints: + - link1_joint + - link2_joint + - link3_joint + - link4_joint + - link5_joint + - link6_joint +initial: # Define initial robot poses. + - group: arm +# pose: home diff --git a/src/wroboarm_21/config/joint_limits.yaml b/src/wroboarm_21/config/joint_limits.yaml new file mode 100644 index 00000000..42cd7faf --- /dev/null +++ b/src/wroboarm_21/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + link1_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + link2_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + link3_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + link4_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + link5_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + link6_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/src/wroboarm_21/config/joint_names_wroboarm_21.yaml b/src/wroboarm_21/config/joint_names_wroboarm_21.yaml new file mode 100644 index 00000000..41ccc5d4 --- /dev/null +++ b/src/wroboarm_21/config/joint_names_wroboarm_21.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'link1_joint', 'link2_joint', 'link3_joint', 'link4_joint', 'link5_joint', 'link6_joint', ] diff --git a/src/wroboarm_21/config/kinematics.yaml b/src/wroboarm_21/config/kinematics.yaml new file mode 100644 index 00000000..05573e5c --- /dev/null +++ b/src/wroboarm_21/config/kinematics.yaml @@ -0,0 +1,4 @@ +arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/src/wroboarm_21/config/ompl_planning.yaml b/src/wroboarm_21/config/ompl_planning.yaml new file mode 100644 index 00000000..ce098a0c --- /dev/null +++ b/src/wroboarm_21/config/ompl_planning.yaml @@ -0,0 +1,156 @@ +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: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + 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: + 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: + 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: + 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: + 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: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + 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: + 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: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + 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: + 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: + type: geometric::PDST + STRIDE: + 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: + 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: + 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: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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: + 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 +arm: + default_planner_config: RRTstar + 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 \ No newline at end of file diff --git a/src/wroboarm_21/config/ros_controllers.yaml b/src/wroboarm_21/config/ros_controllers.yaml new file mode 100644 index 00000000..dc77ec42 --- /dev/null +++ b/src/wroboarm_21/config/ros_controllers.yaml @@ -0,0 +1,35 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: todo_group_name + joint_model_group_pose: todo_state_name +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - link1_joint + - link2_joint + - link3_joint + - link4_joint + - link5_joint + - link6_joint + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - link1_joint + - link2_joint + - link3_joint + - link4_joint + - link5_joint + - link6_joint \ No newline at end of file diff --git a/src/wroboarm_21/config/sensors_3d.yaml b/src/wroboarm_21/config/sensors_3d.yaml new file mode 100644 index 00000000..d2955dcd --- /dev/null +++ b/src/wroboarm_21/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/src/wroboarm_21/config/wroboarm_21.srdf b/src/wroboarm_21/config/wroboarm_21.srdf new file mode 100644 index 00000000..4b9ab407 --- /dev/null +++ b/src/wroboarm_21/config/wroboarm_21.srdf @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/export.log b/src/wroboarm_21/export.log new file mode 100644 index 00000000..9dafc806 --- /dev/null +++ b/src/wroboarm_21/export.log @@ -0,0 +1,365 @@ +2020-12-21 21:22:47,283 INFO Logger.cs: 70 - +-------------------------------------------------------------------------------- +2020-12-21 21:22:47,333 INFO Logger.cs: 71 - Logging commencing for SW2URDF exporter +2020-12-21 21:22:47,335 INFO Logger.cs: 73 - Commit version 1.6.0-1-g15f4949 +2020-12-21 21:22:47,336 INFO Logger.cs: 74 - Build version 1.6.7594.29634 +2020-12-21 21:22:47,339 INFO SwAddin.cs: 192 - Attempting to connect to SW +2020-12-21 21:22:47,340 INFO SwAddin.cs: 197 - Setting up callbacks +2020-12-21 21:22:47,341 INFO SwAddin.cs: 201 - Setting up command manager +2020-12-21 21:22:47,343 INFO SwAddin.cs: 204 - Adding command manager +2020-12-21 21:22:47,347 INFO SwAddin.cs: 263 - Adding Assembly export to file menu +2020-12-21 21:22:47,349 INFO SwAddin.cs: 272 - Adding Part export to file menu +2020-12-21 21:22:47,350 INFO SwAddin.cs: 210 - Adding event handlers +2020-12-21 21:22:47,354 INFO SwAddin.cs: 217 - Connecting plugin to SolidWorks +2020-12-21 21:24:27,526 INFO SwAddin.cs: 294 - Assembly export called for file SimpleArm +2020-12-21 21:24:29,650 INFO SwAddin.cs: 313 - Saving assembly +2020-12-21 21:24:29,826 INFO SwAddin.cs: 316 - Opening property manager +2020-12-21 21:24:29,876 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from SimpleArm +2020-12-21 21:24:29,891 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm +2020-12-21 21:24:29,893 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in SimpleArm +2020-12-21 21:24:29,894 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2020-12-21 21:24:29,896 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2020-12-21 21:24:29,898 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forearm-1 +2020-12-21 21:24:29,900 INFO ExportHelperExtension.cs: 1136 - Found 31 in Forearm-1 +2020-12-21 21:24:29,901 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forearm-1 +2020-12-21 21:24:29,902 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Elbow-1 +2020-12-21 21:24:29,904 INFO ExportHelperExtension.cs: 1136 - Found 27 in Elbow-1 +2020-12-21 21:24:29,905 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Elbow-1 +2020-12-21 21:24:29,906 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from TurnTable-1 +2020-12-21 21:24:29,907 INFO ExportHelperExtension.cs: 1136 - Found 25 in TurnTable-1 +2020-12-21 21:24:29,908 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in TurnTable-1 +2020-12-21 21:24:29,909 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2020-12-21 21:24:29,910 INFO ExportHelperExtension.cs: 1136 - Found 27 in UpperArm-1 +2020-12-21 21:24:29,912 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2020-12-21 21:24:29,913 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2020-12-21 21:24:29,915 INFO ExportHelperExtension.cs: 1136 - Found 29 in Shoulder-1 +2020-12-21 21:24:29,916 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2020-12-21 21:24:29,917 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2020-12-21 21:24:29,918 INFO ExportHelperExtension.cs: 1136 - Found 27 in Manipulator-1 +2020-12-21 21:24:29,921 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2020-12-21 21:24:29,924 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2020-12-21 21:24:29,925 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 +2020-12-21 21:24:29,928 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2020-12-21 21:24:29,930 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from SimpleArm +2020-12-21 21:24:29,932 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm +2020-12-21 21:24:29,934 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in SimpleArm +2020-12-21 21:24:29,935 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2020-12-21 21:24:29,936 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2020-12-21 21:24:29,937 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forearm-1 +2020-12-21 21:24:29,938 INFO ExportHelperExtension.cs: 1136 - Found 31 in Forearm-1 +2020-12-21 21:24:29,940 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forearm-1 +2020-12-21 21:24:29,940 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Elbow-1 +2020-12-21 21:24:29,941 INFO ExportHelperExtension.cs: 1136 - Found 27 in Elbow-1 +2020-12-21 21:24:29,943 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Elbow-1 +2020-12-21 21:24:29,944 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from TurnTable-1 +2020-12-21 21:24:29,945 INFO ExportHelperExtension.cs: 1136 - Found 25 in TurnTable-1 +2020-12-21 21:24:29,946 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in TurnTable-1 +2020-12-21 21:24:29,947 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2020-12-21 21:24:29,949 INFO ExportHelperExtension.cs: 1136 - Found 27 in UpperArm-1 +2020-12-21 21:24:29,950 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2020-12-21 21:24:29,952 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2020-12-21 21:24:29,953 INFO ExportHelperExtension.cs: 1136 - Found 29 in Shoulder-1 +2020-12-21 21:24:29,954 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2020-12-21 21:24:29,962 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2020-12-21 21:24:30,065 INFO ExportHelperExtension.cs: 1136 - Found 27 in Manipulator-1 +2020-12-21 21:24:30,066 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2020-12-21 21:24:30,068 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2020-12-21 21:24:30,070 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 +2020-12-21 21:24:30,071 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2020-12-21 21:24:30,351 INFO SwAddin.cs: 339 - Loading config tree +2020-12-21 21:24:30,360 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametrueturntablexyztrue-2.8254193696243868E-170-6.7465241345308752E-18rpytrue000originfalsefalsevaluetrue0.39680623003092352massfalseixxtrue0.00057145415023748434ixytrue0ixztrue2.2383483955223888E-21iyytrue0.0011002410492471767iyztrue0izztrue0.00057145415023748434inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametruegearboxxyztrue-6.8631413935065579E-180.03801532439663572-0.0013892205096921757rpytrue000originfalsefalsevaluetrue0.34120985304638518massfalseixxtrue0.00063726488338474081ixytrue7.0764513023917949E-20ixztrue-2.7252675378665934E-19iyytrue0.00083450439761804886iyztrue1.6294136810805574E-05izztrue0.00088931887892574886inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink1_jointtypetruecontinuousxyztrue000.0127rpytrue1.570800originfalsefalselinktrueturntableparenttruelinktruegearboxchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link1_joint2Origin_link1_joint2linktruenametrueupperarmxyztrue1.4123887173905075E-170.157206632847804122.7755575615628914E-17rpytrue000originfalsefalsevaluetrue1.264124215876306massfalseixxtrue0.013677990391055649ixytrue3.368445392332098E-19ixztrue3.9611983830710858E-20iyytrue0.000964387075304149iyztrue-2.6745670344635044E-18izztrue0.014091749673708917inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink2_jointtypetruecontinuousxyztrue00.072390.03683rpytrue000originfalsefalselinktruegearboxparenttruelinktrueupperarmchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link2_joint2Origin_link2_joint2linktruenametrueelbowxyztrue7.3321042375852733E-180.046279720003911486-0.0099170828579809855rpytrue000originfalsefalsevaluetrue0.625328404239412massfalseixxtrue0.0015093628788121875ixytrue7.2251958937028559E-20ixztrue2.6531119727066055E-20iyytrue0.00077150412582161528iyztrue-1.1346887107722466E-05izztrue0.0010232990551232335inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink3_jointtypetruecontinuousxyztrue00.3810rpytrue-1.570800originfalsefalselinktrueupperarmparenttruelinktrueelbowchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link3_jointOrigin_link3_jointlinktruenametrueforearmxyztrue-1.1893700871757326E-17-1.9081958235744878E-17-0.11306639219877124rpytrue000originfalsefalsevaluetrue0.46063240123192695massfalseixxtrue0.0022362056644483611ixytrue2.8623740668759855E-21ixztrue-6.9238813900453064E-20iyytrue0.0022468403158638628iyztrue-3.0218548775228389E-19izztrue0.00024560226807657237inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink4_jointtypetruecontinuousxyztrue00.044450.04445rpytrue3.141600originfalsefalselinktrueelbowparenttruelinktrueforearmchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link4_jointOrigin_link4_jointlinktruenametruewristxyztrue-7.1276563553462352E-097.8062556418956319E-18-3.4149891803281207E-09rpytrue000originfalsefalsevaluetrue0.064200890136229274massfalseixxtrue3.4980909309943769E-05ixytrue5.2887802729807491E-11ixztrue1.565944720011522E-18iyytrue2.4290927928157113E-05iyztrue-4.3708304367957221E-11izztrue2.8670971509987432E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink5_jointtypetruecontinuousxyztrue00-0.254rpytrue3.141600originfalsefalselinktrueforearmparenttruelinktruewristchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link5_jointOrigin_link5_jointlinktruenametruemanipulatorxyztrue3.4694469519536142E-189.8733856357333089E-18-0.038099999999999912rpytrue000originfalsefalsevaluetrue0.1580336580927709massfalseixxtrue0.00011719996250307502ixytrue2.2033040782480907E-21ixztrue2.5135101783302778E-20iyytrue0.00011719996250307503iyztrue8.5958526221282082E-21izztrue8.1464432723451653E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink6_jointtypetruecontinuousxyztrue000.0254rpytrue3.141601.5708originfalsefalselinktruewristparenttruelinktruemanipulatorchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link6_jointOrigin_link6_jointlinktruefalse4C4AAAUAAAD//v8XTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAA1AAAAfalsefalsefalse4C4AAAUAAAD//v8RVwByAGkAcwB0AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAxAAAAfalsefalsefalse4C4AAAUAAAD//v8TRgBvAHIAZQBhAHIAbQAtADEAQABTAGkAbQBwAGwAZQBBAHIAbQAEAAAAEAAAAAEAAAABAAAAKgAAAA==falsefalsefalse4C4AAAUAAAD//v8RRQBsAGIAbwB3AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAoAAAAfalsefalsefalse4C4AAAUAAAD//v8UVQBwAHAAZQByAEEAcgBtAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAaAAAAfalsefalsefalse4C4AAAUAAAD//v8UUwBoAG8AdQBsAGQAZQByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAYAAAAfalsefalsefalse4C4AAAUAAAD//v8VVAB1AHIAbgBUAGEAYgBsAGUALQAxAEAAUwBpAG0AcABsAGUAQQByAG0ABAAAABAAAAABAAAAAQAAABkAAAA=falsefalse +2020-12-21 21:24:30,482 INFO LinkNode.cs: 35 - Building node turntable +2020-12-21 21:24:30,484 INFO LinkNode.cs: 35 - Building node gearbox +2020-12-21 21:24:30,484 INFO LinkNode.cs: 35 - Building node upperarm +2020-12-21 21:24:30,485 INFO LinkNode.cs: 35 - Building node elbow +2020-12-21 21:24:30,485 INFO LinkNode.cs: 35 - Building node forearm +2020-12-21 21:24:30,486 INFO LinkNode.cs: 35 - Building node wrist +2020-12-21 21:24:30,486 INFO LinkNode.cs: 35 - Building node manipulator +2020-12-21 21:24:30,487 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for turntable from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,489 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???TurnTable-1@SimpleArm +2020-12-21 21:24:30,492 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\TurnTable.SLDPRT +2020-12-21 21:24:30,492 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link turntable +2020-12-21 21:24:30,493 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for gearbox from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,493 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Shoulder-1@SimpleArm +2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Shoulder.SLDPRT +2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link gearbox +2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for upperarm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,495 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???UpperArm-1@SimpleArm +2020-12-21 21:24:30,495 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\UpperArm.SLDPRT +2020-12-21 21:24:30,495 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link upperarm +2020-12-21 21:24:30,496 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for elbow from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,496 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Elbow-1@SimpleArm( +2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Elbow.SLDPRT +2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link elbow +2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for forearm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,498 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Forearm-1@SimpleArm* +2020-12-21 21:24:30,498 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Forearm.SLDPRT +2020-12-21 21:24:30,498 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link forearm +2020-12-21 21:24:30,499 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for wrist from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,499 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Wrist-1@SimpleArm1 +2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Wrist.SLDPRT +2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link wrist +2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for manipulator from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,501 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Manipulator-1@SimpleArm5 +2020-12-21 21:24:30,501 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Manipulator.SLDPRT +2020-12-21 21:24:30,502 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link manipulator +2020-12-21 21:24:30,533 INFO SwAddin.cs: 344 - Showing property manager +2020-12-21 21:24:52,093 INFO ExportPropertyManager.cs: 422 - Configuration saved +2020-12-21 21:24:52,099 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametrueturntablexyztrue-2.8254193696243868E-170-6.7465241345308752E-18rpytrue000originfalsefalsevaluetrue0.39680623003092352massfalseixxtrue0.00057145415023748434ixytrue0ixztrue2.2383483955223888E-21iyytrue0.0011002410492471767iyztrue0izztrue0.00057145415023748434inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametruegearboxxyztrue-6.8631413935065579E-180.03801532439663572-0.0013892205096921757rpytrue000originfalsefalsevaluetrue0.34120985304638518massfalseixxtrue0.00063726488338474081ixytrue7.0764513023917949E-20ixztrue-2.7252675378665934E-19iyytrue0.00083450439761804886iyztrue1.6294136810805574E-05izztrue0.00088931887892574886inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink1_jointtypetruecontinuousxyztrue000.0127rpytrue1.570800originfalsefalselinktrueturntableparenttruelinktruegearboxchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link1_joint2Origin_link1_joint2linktruenametrueupperarmxyztrue1.4123887173905075E-170.157206632847804122.7755575615628914E-17rpytrue000originfalsefalsevaluetrue1.264124215876306massfalseixxtrue0.013677990391055649ixytrue3.368445392332098E-19ixztrue3.9611983830710858E-20iyytrue0.000964387075304149iyztrue-2.6745670344635044E-18izztrue0.014091749673708917inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink2_jointtypetruecontinuousxyztrue00.072390.03683rpytrue000originfalsefalselinktruegearboxparenttruelinktrueupperarmchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link2_joint2Origin_link2_joint2linktruenametrueelbowxyztrue7.3321042375852733E-180.046279720003911486-0.0099170828579809855rpytrue000originfalsefalsevaluetrue0.625328404239412massfalseixxtrue0.0015093628788121875ixytrue7.2251958937028559E-20ixztrue2.6531119727066055E-20iyytrue0.00077150412582161528iyztrue-1.1346887107722466E-05izztrue0.0010232990551232335inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink3_jointtypetruecontinuousxyztrue00.3810rpytrue-1.570800originfalsefalselinktrueupperarmparenttruelinktrueelbowchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link3_jointOrigin_link3_jointlinktruenametrueforearmxyztrue-1.1893700871757326E-17-1.9081958235744878E-17-0.11306639219877124rpytrue000originfalsefalsevaluetrue0.46063240123192695massfalseixxtrue0.0022362056644483611ixytrue2.8623740668759855E-21ixztrue-6.9238813900453064E-20iyytrue0.0022468403158638628iyztrue-3.0218548775228389E-19izztrue0.00024560226807657237inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink4_jointtypetruecontinuousxyztrue00.044450.04445rpytrue3.141600originfalsefalselinktrueelbowparenttruelinktrueforearmchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link4_jointOrigin_link4_jointlinktruenametruewristxyztrue-7.1276563553462352E-097.8062556418956319E-18-3.4149891803281207E-09rpytrue000originfalsefalsevaluetrue0.064200890136229274massfalseixxtrue3.4980909309943769E-05ixytrue5.2887802729807491E-11ixztrue1.565944720011522E-18iyytrue2.4290927928157113E-05iyztrue-4.3708304367957221E-11izztrue2.8670971509987432E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink5_jointtypetruecontinuousxyztrue00-0.254rpytrue3.141600originfalsefalselinktrueforearmparenttruelinktruewristchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link5_jointOrigin_link5_jointlinktruenametruemanipulatorxyztrue3.4694469519536142E-189.8733856357333089E-18-0.038099999999999912rpytrue000originfalsefalsevaluetrue0.1580336580927709massfalseixxtrue0.00011719996250307502ixytrue2.2033040782480907E-21ixztrue2.5135101783302778E-20iyytrue0.00011719996250307503iyztrue8.5958526221282082E-21izztrue8.1464432723451653E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink6_jointtypetruecontinuousxyztrue000.0254rpytrue3.141601.5708originfalsefalselinktruewristparenttruelinktruemanipulatorchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link6_jointOrigin_link6_jointlinktruefalse4C4AAAUAAAD//v8XTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAA1AAAAfalsefalsefalse4C4AAAUAAAD//v8RVwByAGkAcwB0AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAxAAAAfalsefalsefalse4C4AAAUAAAD//v8TRgBvAHIAZQBhAHIAbQAtADEAQABTAGkAbQBwAGwAZQBBAHIAbQAEAAAAEAAAAAEAAAABAAAAKgAAAA==falsefalsefalse4C4AAAUAAAD//v8RRQBsAGIAbwB3AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAoAAAAfalsefalsefalse4C4AAAUAAAD//v8UVQBwAHAAZQByAEEAcgBtAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAaAAAAfalsefalsefalse4C4AAAUAAAD//v8UUwBoAG8AdQBsAGQAZQByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAYAAAAfalsefalsefalse4C4AAAUAAAD//v8VVAB1AHIAbgBUAGEAYgBsAGUALQAxAEAAUwBpAG0AcABsAGUAQQByAG0ABAAAABAAAAABAAAAAQAAABkAAAA=falsefalse +2020-12-21 21:24:52,272 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2020-12-21 21:26:19,586 INFO SwAddin.cs: 294 - Assembly export called for file SimpleArm +2020-12-21 21:26:19,586 INFO SwAddin.cs: 299 - Save is required +2020-12-21 21:26:19,587 INFO SwAddin.cs: 313 - Saving assembly +2020-12-21 21:26:20,090 INFO SwAddin.cs: 316 - Opening property manager +2020-12-21 21:26:20,091 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from SimpleArm +2020-12-21 21:26:20,092 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm +2020-12-21 21:26:20,093 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in SimpleArm +2020-12-21 21:26:20,094 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2020-12-21 21:26:20,095 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2020-12-21 21:26:20,096 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forearm-1 +2020-12-21 21:26:20,097 INFO ExportHelperExtension.cs: 1136 - Found 31 in Forearm-1 +2020-12-21 21:26:20,098 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forearm-1 +2020-12-21 21:26:20,099 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Elbow-1 +2020-12-21 21:26:20,100 INFO ExportHelperExtension.cs: 1136 - Found 27 in Elbow-1 +2020-12-21 21:26:20,101 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Elbow-1 +2020-12-21 21:26:20,103 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from TurnTable-1 +2020-12-21 21:26:20,104 INFO ExportHelperExtension.cs: 1136 - Found 25 in TurnTable-1 +2020-12-21 21:26:20,105 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in TurnTable-1 +2020-12-21 21:26:20,106 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2020-12-21 21:26:20,108 INFO ExportHelperExtension.cs: 1136 - Found 27 in UpperArm-1 +2020-12-21 21:26:20,109 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2020-12-21 21:26:20,110 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2020-12-21 21:26:20,111 INFO ExportHelperExtension.cs: 1136 - Found 29 in Shoulder-1 +2020-12-21 21:26:20,113 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2020-12-21 21:26:20,115 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2020-12-21 21:26:20,116 INFO ExportHelperExtension.cs: 1136 - Found 27 in Manipulator-1 +2020-12-21 21:26:20,122 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2020-12-21 21:26:20,123 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2020-12-21 21:26:20,125 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 +2020-12-21 21:26:20,127 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2020-12-21 21:26:20,128 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from SimpleArm +2020-12-21 21:26:20,129 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm +2020-12-21 21:26:20,130 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in SimpleArm +2020-12-21 21:26:20,133 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2020-12-21 21:26:20,134 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2020-12-21 21:26:20,135 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forearm-1 +2020-12-21 21:26:20,136 INFO ExportHelperExtension.cs: 1136 - Found 31 in Forearm-1 +2020-12-21 21:26:20,137 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forearm-1 +2020-12-21 21:26:20,138 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Elbow-1 +2020-12-21 21:26:20,139 INFO ExportHelperExtension.cs: 1136 - Found 27 in Elbow-1 +2020-12-21 21:26:20,149 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Elbow-1 +2020-12-21 21:26:20,151 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from TurnTable-1 +2020-12-21 21:26:20,231 INFO ExportHelperExtension.cs: 1136 - Found 25 in TurnTable-1 +2020-12-21 21:26:20,233 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in TurnTable-1 +2020-12-21 21:26:20,234 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2020-12-21 21:26:20,235 INFO ExportHelperExtension.cs: 1136 - Found 27 in UpperArm-1 +2020-12-21 21:26:20,238 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2020-12-21 21:26:20,239 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2020-12-21 21:26:20,240 INFO ExportHelperExtension.cs: 1136 - Found 29 in Shoulder-1 +2020-12-21 21:26:20,241 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2020-12-21 21:26:20,242 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2020-12-21 21:26:20,243 INFO ExportHelperExtension.cs: 1136 - Found 27 in Manipulator-1 +2020-12-21 21:26:20,244 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2020-12-21 21:26:20,244 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2020-12-21 21:26:20,247 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 +2020-12-21 21:26:20,248 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2020-12-21 21:26:20,353 INFO SwAddin.cs: 339 - Loading config tree +2020-12-21 21:26:20,357 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametrueturntablexyztrue-2.8254193696243868E-170-6.7465241345308752E-18rpytrue000originfalsefalsevaluetrue0.39680623003092352massfalseixxtrue0.00057145415023748434ixytrue0ixztrue2.2383483955223888E-21iyytrue0.0011002410492471767iyztrue0izztrue0.00057145415023748434inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametruegearboxxyztrue-6.8631413935065579E-180.03801532439663572-0.0013892205096921757rpytrue000originfalsefalsevaluetrue0.34120985304638518massfalseixxtrue0.00063726488338474081ixytrue7.0764513023917949E-20ixztrue-2.7252675378665934E-19iyytrue0.00083450439761804886iyztrue1.6294136810805574E-05izztrue0.00088931887892574886inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink1_jointtypetruecontinuousxyztrue000.0127rpytrue1.570800originfalsefalselinktrueturntableparenttruelinktruegearboxchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueupperarmxyztrue1.4123887173905075E-170.157206632847804122.7755575615628914E-17rpytrue000originfalsefalsevaluetrue1.264124215876306massfalseixxtrue0.013677990391055649ixytrue3.368445392332098E-19ixztrue3.9611983830710858E-20iyytrue0.000964387075304149iyztrue-2.6745670344635044E-18izztrue0.014091749673708917inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink2_jointtypetruecontinuousxyztrue00.072390.03683rpytrue000originfalsefalselinktruegearboxparenttruelinktrueupperarmchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueelbowxyztrue7.3321042375852733E-180.046279720003911486-0.0099170828579809855rpytrue000originfalsefalsevaluetrue0.625328404239412massfalseixxtrue0.0015093628788121875ixytrue7.2251958937028559E-20ixztrue2.6531119727066055E-20iyytrue0.00077150412582161528iyztrue-1.1346887107722466E-05izztrue0.0010232990551232335inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink3_jointtypetruecontinuousxyztrue00.3810rpytrue-1.570800originfalsefalselinktrueupperarmparenttruelinktrueelbowchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link3_jointOrigin_link3_jointlinktruenametrueforearmxyztrue-1.1893700871757326E-17-1.9081958235744878E-17-0.11306639219877124rpytrue000originfalsefalsevaluetrue0.46063240123192695massfalseixxtrue0.0022362056644483611ixytrue2.8623740668759855E-21ixztrue-6.9238813900453064E-20iyytrue0.0022468403158638628iyztrue-3.0218548775228389E-19izztrue0.00024560226807657237inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink4_jointtypetruecontinuousxyztrue00.044450.04445rpytrue3.141600originfalsefalselinktrueelbowparenttruelinktrueforearmchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link4_jointOrigin_link4_jointlinktruenametruewristxyztrue-7.1276563553462352E-097.8062556418956319E-18-3.4149891803281207E-09rpytrue000originfalsefalsevaluetrue0.064200890136229274massfalseixxtrue3.4980909309943769E-05ixytrue5.2887802729807491E-11ixztrue1.565944720011522E-18iyytrue2.4290927928157113E-05iyztrue-4.3708304367957221E-11izztrue2.8670971509987432E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink5_jointtypetruecontinuousxyztrue00-0.254rpytrue3.141600originfalsefalselinktrueforearmparenttruelinktruewristchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link5_jointOrigin_link5_jointlinktruenametruemanipulatorxyztrue3.4694469519536142E-189.8733856357333089E-18-0.038099999999999912rpytrue000originfalsefalsevaluetrue0.1580336580927709massfalseixxtrue0.00011719996250307502ixytrue2.2033040782480907E-21ixztrue2.5135101783302778E-20iyytrue0.00011719996250307503iyztrue8.5958526221282082E-21izztrue8.1464432723451653E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink6_jointtypetruecontinuousxyztrue000.0254rpytrue3.141601.5708originfalsefalselinktruewristparenttruelinktruemanipulatorchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link6_jointOrigin_link6_jointlinktruefalseyDIAAAUAAAD//v8XTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAA1AAAAfalsefalsefalseyDIAAAUAAAD//v8RVwByAGkAcwB0AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAxAAAAfalsefalsefalseyDIAAAUAAAD//v8TRgBvAHIAZQBhAHIAbQAtADEAQABTAGkAbQBwAGwAZQBBAHIAbQAEAAAAEAAAAAEAAAABAAAAKgAAAA==falsefalsefalseyDIAAAUAAAD//v8RRQBsAGIAbwB3AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAoAAAAfalsefalsefalseyDIAAAUAAAD//v8UVQBwAHAAZQByAEEAcgBtAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAaAAAAfalsefalsefalseyDIAAAUAAAD//v8UUwBoAG8AdQBsAGQAZQByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAYAAAAfalsefalsefalseyDIAAAUAAAD//v8VVAB1AHIAbgBUAGEAYgBsAGUALQAxAEAAUwBpAG0AcABsAGUAQQByAG0ABAAAABAAAAABAAAAAQAAABkAAAA=falsefalse +2020-12-21 21:26:20,365 INFO LinkNode.cs: 35 - Building node turntable +2020-12-21 21:26:20,366 INFO LinkNode.cs: 35 - Building node gearbox +2020-12-21 21:26:20,366 INFO LinkNode.cs: 35 - Building node upperarm +2020-12-21 21:26:20,367 INFO LinkNode.cs: 35 - Building node elbow +2020-12-21 21:26:20,368 INFO LinkNode.cs: 35 - Building node forearm +2020-12-21 21:26:20,369 INFO LinkNode.cs: 35 - Building node wrist +2020-12-21 21:26:20,369 INFO LinkNode.cs: 35 - Building node manipulator +2020-12-21 21:26:20,370 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for turntable from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,371 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???TurnTable-1@SimpleArm +2020-12-21 21:26:20,372 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\TurnTable.SLDPRT +2020-12-21 21:26:20,373 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link turntable +2020-12-21 21:26:20,374 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for gearbox from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,374 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Shoulder-1@SimpleArm +2020-12-21 21:26:20,376 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Shoulder.SLDPRT +2020-12-21 21:26:20,376 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link gearbox +2020-12-21 21:26:20,377 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for upperarm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,378 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???UpperArm-1@SimpleArm +2020-12-21 21:26:20,380 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\UpperArm.SLDPRT +2020-12-21 21:26:20,381 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link upperarm +2020-12-21 21:26:20,381 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for elbow from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,382 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Elbow-1@SimpleArm( +2020-12-21 21:26:20,383 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Elbow.SLDPRT +2020-12-21 21:26:20,384 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link elbow +2020-12-21 21:26:20,385 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for forearm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,386 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Forearm-1@SimpleArm* +2020-12-21 21:26:20,387 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Forearm.SLDPRT +2020-12-21 21:26:20,388 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link forearm +2020-12-21 21:26:20,390 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for wrist from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,391 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Wrist-1@SimpleArm1 +2020-12-21 21:26:20,392 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Wrist.SLDPRT +2020-12-21 21:26:20,393 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link wrist +2020-12-21 21:26:20,395 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for manipulator from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,395 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Manipulator-1@SimpleArm5 +2020-12-21 21:26:20,397 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Manipulator.SLDPRT +2020-12-21 21:26:20,398 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link manipulator +2020-12-21 21:26:20,427 INFO SwAddin.cs: 344 - Showing property manager +2020-12-21 21:26:35,601 INFO ExportPropertyManager.cs: 422 - Configuration saved +2020-12-21 21:26:35,603 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametrueturntablexyztrue-2.8254193696243868E-170-6.7465241345308752E-18rpytrue000originfalsefalsevaluetrue0.39680623003092352massfalseixxtrue0.00057145415023748434ixytrue0ixztrue2.2383483955223888E-21iyytrue0.0011002410492471767iyztrue0izztrue0.00057145415023748434inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametruegearboxxyztrue-6.8631413935065579E-180.03801532439663572-0.0013892205096921757rpytrue000originfalsefalsevaluetrue0.34120985304638518massfalseixxtrue0.00063726488338474081ixytrue7.0764513023917949E-20ixztrue-2.7252675378665934E-19iyytrue0.00083450439761804886iyztrue1.6294136810805574E-05izztrue0.00088931887892574886inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink1_jointtypetruecontinuousxyztrue000.0127rpytrue1.570800originfalsefalselinktrueturntableparenttruelinktruegearboxchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueupperarmxyztrue1.4123887173905075E-170.157206632847804122.7755575615628914E-17rpytrue000originfalsefalsevaluetrue1.264124215876306massfalseixxtrue0.013677990391055649ixytrue3.368445392332098E-19ixztrue3.9611983830710858E-20iyytrue0.000964387075304149iyztrue-2.6745670344635044E-18izztrue0.014091749673708917inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink2_jointtypetruecontinuousxyztrue00.072390.03683rpytrue000originfalsefalselinktruegearboxparenttruelinktrueupperarmchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueelbowxyztrue7.3321042375852733E-180.046279720003911486-0.0099170828579809855rpytrue000originfalsefalsevaluetrue0.625328404239412massfalseixxtrue0.0015093628788121875ixytrue7.2251958937028559E-20ixztrue2.6531119727066055E-20iyytrue0.00077150412582161528iyztrue-1.1346887107722466E-05izztrue0.0010232990551232335inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink3_jointtypetruecontinuousxyztrue00.3810rpytrue-1.570800originfalsefalselinktrueupperarmparenttruelinktrueelbowchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link3_jointOrigin_link3_jointlinktruenametrueforearmxyztrue-1.1893700871757326E-17-1.9081958235744878E-17-0.11306639219877124rpytrue000originfalsefalsevaluetrue0.46063240123192695massfalseixxtrue0.0022362056644483611ixytrue2.8623740668759855E-21ixztrue-6.9238813900453064E-20iyytrue0.0022468403158638628iyztrue-3.0218548775228389E-19izztrue0.00024560226807657237inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink4_jointtypetruecontinuousxyztrue00.044450.04445rpytrue3.141600originfalsefalselinktrueelbowparenttruelinktrueforearmchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link4_jointOrigin_link4_jointlinktruenametruewristxyztrue-7.1276563553462352E-097.8062556418956319E-18-3.4149891803281207E-09rpytrue000originfalsefalsevaluetrue0.064200890136229274massfalseixxtrue3.4980909309943769E-05ixytrue5.2887802729807491E-11ixztrue1.565944720011522E-18iyytrue2.4290927928157113E-05iyztrue-4.3708304367957221E-11izztrue2.8670971509987432E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink5_jointtypetruecontinuousxyztrue00-0.254rpytrue3.141600originfalsefalselinktrueforearmparenttruelinktruewristchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link5_jointOrigin_link5_jointlinktruenametruemanipulatorxyztrue3.4694469519536142E-189.8733856357333089E-18-0.038099999999999912rpytrue000originfalsefalsevaluetrue0.1580336580927709massfalseixxtrue0.00011719996250307502ixytrue2.2033040782480907E-21ixztrue2.5135101783302778E-20iyytrue0.00011719996250307503iyztrue8.5958526221282082E-21izztrue8.1464432723451653E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink6_jointtypetruecontinuousxyztrue000.0254rpytrue3.141601.5708originfalsefalselinktruewristparenttruelinktruemanipulatorchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link6_jointOrigin_link6_jointlinktruefalseyDIAAAUAAAD//v8XTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAA1AAAAfalsefalsefalseyDIAAAUAAAD//v8RVwByAGkAcwB0AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAxAAAAfalsefalsefalseyDIAAAUAAAD//v8TRgBvAHIAZQBhAHIAbQAtADEAQABTAGkAbQBwAGwAZQBBAHIAbQAEAAAAEAAAAAEAAAABAAAAKgAAAA==falsefalsefalseyDIAAAUAAAD//v8RRQBsAGIAbwB3AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAoAAAAfalsefalsefalseyDIAAAUAAAD//v8UVQBwAHAAZQByAEEAcgBtAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAaAAAAfalsefalsefalseyDIAAAUAAAD//v8UUwBoAG8AdQBsAGQAZQByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAYAAAAfalsefalsefalseyDIAAAUAAAD//v8VVAB1AHIAbgBUAGEAYgBsAGUALQAxAEAAUwBpAG0AcABsAGUAQQByAG0ABAAAABAAAAABAAAAAQAAABkAAAA=falsefalse +2020-12-21 21:26:35,744 INFO ExportHelperExtension.cs: 347 - Creating joint gearbox +2020-12-21 21:26:35,753 INFO ExportHelperExtension.cs: 1397 - Fixing components for turntable +2020-12-21 21:26:35,754 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2020-12-21 21:26:36,153 INFO : 0 - R1: 1, System.__ComObject, 1, System.Double[] +2020-12-21 21:26:36,158 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2020-12-21 21:26:36,158 INFO ExportHelperExtension.cs: 849 - L1: 0 +2020-12-21 21:26:36,159 INFO ExportHelperExtension.cs: 857 - L2: 0 +2020-12-21 21:26:36,298 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2020-12-21 21:26:39,178 WARN ExportHelperExtension.cs: 351 - Creating joint from parent turntable to child gearbox failed +2020-12-21 21:26:39,185 INFO ExportHelperExtension.cs: 347 - Creating joint upperarm +2020-12-21 21:26:39,186 INFO ExportHelperExtension.cs: 1397 - Fixing components for gearbox +2020-12-21 21:26:39,186 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2020-12-21 21:26:39,186 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2020-12-21 21:26:39,481 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2020-12-21 21:26:39,482 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2020-12-21 21:26:39,483 INFO ExportHelperExtension.cs: 849 - L1: 0 +2020-12-21 21:26:39,483 INFO ExportHelperExtension.cs: 857 - L2: 0 +2020-12-21 21:26:39,484 INFO ExportHelperExtension.cs: 1352 - Unfixing component 24 +2020-12-21 21:26:39,484 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2020-12-21 21:26:42,112 WARN ExportHelperExtension.cs: 351 - Creating joint from parent gearbox to child upperarm failed +2020-12-21 21:26:42,118 INFO ExportHelperExtension.cs: 347 - Creating joint elbow +2020-12-21 21:26:42,166 WARN ExportHelperExtension.cs: 351 - Creating joint from parent upperarm to child elbow failed +2020-12-21 21:26:42,172 INFO ExportHelperExtension.cs: 347 - Creating joint forearm +2020-12-21 21:26:42,224 WARN ExportHelperExtension.cs: 351 - Creating joint from parent elbow to child forearm failed +2020-12-21 21:26:42,231 INFO ExportHelperExtension.cs: 347 - Creating joint wrist +2020-12-21 21:26:42,283 WARN ExportHelperExtension.cs: 351 - Creating joint from parent forearm to child wrist failed +2020-12-21 21:26:42,290 INFO ExportHelperExtension.cs: 347 - Creating joint manipulator +2020-12-21 21:26:42,346 WARN ExportHelperExtension.cs: 351 - Creating joint from parent wrist to child manipulator failed +2020-12-21 21:26:42,543 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from SimpleArm +2020-12-21 21:26:42,552 INFO ExportHelperExtension.cs: 1136 - Found 63 in SimpleArm +2020-12-21 21:26:42,553 INFO ExportHelperExtension.cs: 1145 - Found 9 features of type [CoordSys] in SimpleArm +2020-12-21 21:26:42,554 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2020-12-21 21:26:42,554 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2020-12-21 21:26:42,554 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forearm-1 +2020-12-21 21:26:42,555 INFO ExportHelperExtension.cs: 1136 - Found 31 in Forearm-1 +2020-12-21 21:26:42,555 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forearm-1 +2020-12-21 21:26:42,556 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Elbow-1 +2020-12-21 21:26:42,556 INFO ExportHelperExtension.cs: 1136 - Found 27 in Elbow-1 +2020-12-21 21:26:42,557 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Elbow-1 +2020-12-21 21:26:42,557 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from TurnTable-1 +2020-12-21 21:26:42,558 INFO ExportHelperExtension.cs: 1136 - Found 25 in TurnTable-1 +2020-12-21 21:26:42,558 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in TurnTable-1 +2020-12-21 21:26:42,559 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2020-12-21 21:26:42,560 INFO ExportHelperExtension.cs: 1136 - Found 27 in UpperArm-1 +2020-12-21 21:26:42,560 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2020-12-21 21:26:42,561 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2020-12-21 21:26:42,561 INFO ExportHelperExtension.cs: 1136 - Found 29 in Shoulder-1 +2020-12-21 21:26:42,562 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2020-12-21 21:26:42,562 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2020-12-21 21:26:42,563 INFO ExportHelperExtension.cs: 1136 - Found 27 in Manipulator-1 +2020-12-21 21:26:42,563 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2020-12-21 21:26:42,563 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2020-12-21 21:26:42,564 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 +2020-12-21 21:26:42,564 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2020-12-21 21:26:42,565 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from SimpleArm +2020-12-21 21:26:42,565 INFO ExportHelperExtension.cs: 1136 - Found 63 in SimpleArm +2020-12-21 21:26:42,566 INFO ExportHelperExtension.cs: 1145 - Found 8 features of type [RefAxis] in SimpleArm +2020-12-21 21:26:42,566 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2020-12-21 21:26:42,569 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2020-12-21 21:26:42,570 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forearm-1 +2020-12-21 21:26:42,598 INFO ExportHelperExtension.cs: 1136 - Found 31 in Forearm-1 +2020-12-21 21:26:42,599 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forearm-1 +2020-12-21 21:26:42,599 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Elbow-1 +2020-12-21 21:26:42,599 INFO ExportHelperExtension.cs: 1136 - Found 27 in Elbow-1 +2020-12-21 21:26:42,599 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Elbow-1 +2020-12-21 21:26:42,600 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from TurnTable-1 +2020-12-21 21:26:42,600 INFO ExportHelperExtension.cs: 1136 - Found 25 in TurnTable-1 +2020-12-21 21:26:42,601 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in TurnTable-1 +2020-12-21 21:26:42,601 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2020-12-21 21:26:42,602 INFO ExportHelperExtension.cs: 1136 - Found 27 in UpperArm-1 +2020-12-21 21:26:42,602 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2020-12-21 21:26:42,603 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2020-12-21 21:26:42,603 INFO ExportHelperExtension.cs: 1136 - Found 29 in Shoulder-1 +2020-12-21 21:26:42,604 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2020-12-21 21:26:42,604 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2020-12-21 21:26:42,605 INFO ExportHelperExtension.cs: 1136 - Found 27 in Manipulator-1 +2020-12-21 21:26:42,605 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2020-12-21 21:26:42,606 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2020-12-21 21:26:42,608 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 +2020-12-21 21:26:42,609 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2020-12-21 21:26:42,695 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2020-12-21 21:27:18,266 INFO AssemblyExportForm.cs: 253 - Completing URDF export +2020-12-21 21:27:18,273 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametrueturntablexyztrue-2.8254193696243868E-170-6.7465241345308752E-18rpytrue000originfalsefalsevaluetrue0.39680623003092352massfalseixxtrue0.00057145415023748434ixytrue0ixztrue2.2383483955223888E-21iyytrue0.0011002410492471767iyztrue0izztrue0.00057145415023748434inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametruegearboxxyztrue-6.8631413935065579E-180.03801532439663572-0.0013892205096921757rpytrue000originfalsefalsevaluetrue0.34120985304638518massfalseixxtrue0.00063726488338474081ixytrue7.0764513023917949E-20ixztrue-2.7252675378665934E-19iyytrue0.00083450439761804886iyztrue1.6294136810805574E-05izztrue0.00088931887892574886inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink1_jointtypetruecontinuousxyztrue000.0127rpytrue1.570800originfalsefalselinktrueturntableparenttruelinktruegearboxchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueupperarmxyztrue1.4123887173905075E-170.157206632847804122.7755575615628914E-17rpytrue000originfalsefalsevaluetrue1.264124215876306massfalseixxtrue0.013677990391055649ixytrue3.368445392332098E-19ixztrue3.9611983830710858E-20iyytrue0.000964387075304149iyztrue-2.6745670344635044E-18izztrue0.014091749673708917inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink2_jointtypetruecontinuousxyztrue00.072390.03683rpytrue000originfalsefalselinktruegearboxparenttruelinktrueupperarmchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueelbowxyztrue7.3321042375852733E-180.046279720003911486-0.0099170828579809855rpytrue000originfalsefalsevaluetrue0.625328404239412massfalseixxtrue0.0015093628788121875ixytrue7.2251958937028559E-20ixztrue2.6531119727066055E-20iyytrue0.00077150412582161528iyztrue-1.1346887107722466E-05izztrue0.0010232990551232335inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink3_jointtypetruecontinuousxyztrue00.3810rpytrue-1.570800originfalsefalselinktrueupperarmparenttruelinktrueelbowchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link3_jointOrigin_link3_jointlinktruenametrueforearmxyztrue-1.1893700871757326E-17-1.9081958235744878E-17-0.11306639219877124rpytrue000originfalsefalsevaluetrue0.46063240123192695massfalseixxtrue0.0022362056644483611ixytrue2.8623740668759855E-21ixztrue-6.9238813900453064E-20iyytrue0.0022468403158638628iyztrue-3.0218548775228389E-19izztrue0.00024560226807657237inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink4_jointtypetruecontinuousxyztrue00.044450.04445rpytrue3.141600originfalsefalselinktrueelbowparenttruelinktrueforearmchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link4_jointOrigin_link4_jointlinktruenametruewristxyztrue-7.1276563553462352E-097.8062556418956319E-18-3.4149891803281207E-09rpytrue000originfalsefalsevaluetrue0.064200890136229274massfalseixxtrue3.4980909309943769E-05ixytrue5.2887802729807491E-11ixztrue1.565944720011522E-18iyytrue2.4290927928157113E-05iyztrue-4.3708304367957221E-11izztrue2.8670971509987432E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink5_jointtypetruecontinuousxyztrue00-0.254rpytrue3.141600originfalsefalselinktrueforearmparenttruelinktruewristchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link5_jointOrigin_link5_jointlinktruenametruemanipulatorxyztrue3.4694469519536142E-189.8733856357333089E-18-0.038099999999999912rpytrue000originfalsefalsevaluetrue0.1580336580927709massfalseixxtrue0.00011719996250307502ixytrue2.2033040782480907E-21ixztrue2.5135101783302778E-20iyytrue0.00011719996250307503iyztrue8.5958526221282082E-21izztrue8.1464432723451653E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink6_jointtypetruecontinuousxyztrue000.0254rpytrue3.141601.5708originfalsefalselinktruewristparenttruelinktruemanipulatorchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link6_jointOrigin_link6_jointlinktruefalseyDIAAAUAAAD//v8XTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAA1AAAAfalsefalsefalseyDIAAAUAAAD//v8RVwByAGkAcwB0AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAxAAAAfalsefalsefalseyDIAAAUAAAD//v8TRgBvAHIAZQBhAHIAbQAtADEAQABTAGkAbQBwAGwAZQBBAHIAbQAEAAAAEAAAAAEAAAABAAAAKgAAAA==falsefalsefalseyDIAAAUAAAD//v8RRQBsAGIAbwB3AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAoAAAAfalsefalsefalseyDIAAAUAAAD//v8UVQBwAHAAZQByAEEAcgBtAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAaAAAAfalsefalsefalseyDIAAAUAAAD//v8UUwBoAG8AdQBsAGQAZQByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAYAAAAfalsefalsefalseyDIAAAUAAAD//v8VVAB1AHIAbgBUAGEAYgBsAGUALQAxAEAAUwBpAG0AcABsAGUAQQByAG0ABAAAABAAAAABAAAAAQAAABkAAAA=falsefalse +2020-12-21 21:27:31,234 INFO AssemblyExportForm.cs: 309 - Saving URDF package to C:\Users\ustuner\Desktop\wroboarm_21 +2020-12-21 21:27:31,236 INFO ExportHelper.cs: 147 - Beginning the export process +2020-12-21 21:27:31,238 INFO ExportHelper.cs: 153 - Creating package directories with name wroboarm_21 and save path C:\Users\ustuner\Desktop +2020-12-21 21:27:32,837 INFO ExportHelper.cs: 162 - Creating CMakeLists.txt at C:\Users\ustuner\Desktop\wroboarm_21\CMakeLists.txt +2020-12-21 21:27:32,845 INFO ExportHelper.cs: 166 - Creating joint names config at C:\Users\ustuner\Desktop\wroboarm_21\config\joint_names_wroboarm_21.yaml +2020-12-21 21:27:32,853 INFO ExportHelper.cs: 170 - Creating package.xml at C:\Users\ustuner\Desktop\wroboarm_21\package.xml +2020-12-21 21:27:32,854 INFO PackageXMLWriter.cs: 21 - Creating package.xml at C:\Users\ustuner\Desktop\wroboarm_21\package.xml +2020-12-21 21:27:32,867 INFO ExportHelper.cs: 177 - Creating RVIZ launch file in C:\Users\ustuner\Desktop\wroboarm_21\launch\ +2020-12-21 21:27:32,876 INFO ExportHelper.cs: 182 - Creating Gazebo launch file in C:\Users\ustuner\Desktop\wroboarm_21\launch\ +2020-12-21 21:27:32,883 INFO ExportHelper.cs: 187 - Saving existing STL preferences +2020-12-21 21:27:32,884 INFO ExportHelper.cs: 450 - Saving users preferences +2020-12-21 21:27:32,886 INFO ExportHelper.cs: 190 - Modifying STL preferences +2020-12-21 21:27:32,886 INFO ExportHelper.cs: 464 - Setting STL preferences +2020-12-21 21:27:32,891 INFO ExportHelper.cs: 196 - Found 0 hidden components +2020-12-21 21:27:32,892 INFO ExportHelper.cs: 197 - Hiding all components +2020-12-21 21:27:33,029 INFO ExportHelper.cs: 204 - Beginning individual files export +2020-12-21 21:27:33,036 INFO ExportHelper.cs: 270 - Exporting link: turntable +2020-12-21 21:27:33,038 INFO ExportHelper.cs: 272 - Link turntable has 1 children +2020-12-21 21:27:33,040 INFO ExportHelper.cs: 270 - Exporting link: gearbox +2020-12-21 21:27:33,041 INFO ExportHelper.cs: 272 - Link gearbox has 1 children +2020-12-21 21:27:33,044 INFO ExportHelper.cs: 270 - Exporting link: upperarm +2020-12-21 21:27:33,044 INFO ExportHelper.cs: 272 - Link upperarm has 1 children +2020-12-21 21:27:33,046 INFO ExportHelper.cs: 270 - Exporting link: elbow +2020-12-21 21:27:33,047 INFO ExportHelper.cs: 272 - Link elbow has 1 children +2020-12-21 21:27:33,049 INFO ExportHelper.cs: 270 - Exporting link: forearm +2020-12-21 21:27:33,050 INFO ExportHelper.cs: 272 - Link forearm has 1 children +2020-12-21 21:27:33,053 INFO ExportHelper.cs: 270 - Exporting link: wrist +2020-12-21 21:27:33,053 INFO ExportHelper.cs: 272 - Link wrist has 1 children +2020-12-21 21:27:33,062 INFO ExportHelper.cs: 270 - Exporting link: manipulator +2020-12-21 21:27:33,063 INFO ExportHelper.cs: 272 - Link manipulator has 0 children +2020-12-21 21:27:33,064 INFO ExportHelper.cs: 317 - manipulator: Exporting STL with coordinate frame Origin_link6_joint +2020-12-21 21:27:33,065 INFO ExportHelper.cs: 322 - manipulator: Reference geometry name +2020-12-21 21:27:33,151 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\manipulator.STL +2020-12-21 21:27:33,492 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:33,500 INFO ExportHelper.cs: 317 - wrist: Exporting STL with coordinate frame Origin_link5_joint +2020-12-21 21:27:33,501 INFO ExportHelper.cs: 322 - wrist: Reference geometry name +2020-12-21 21:27:33,526 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\wrist.STL +2020-12-21 21:27:33,599 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:33,610 INFO ExportHelper.cs: 317 - forearm: Exporting STL with coordinate frame Origin_link4_joint +2020-12-21 21:27:33,611 INFO ExportHelper.cs: 322 - forearm: Reference geometry name +2020-12-21 21:27:33,629 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\forearm.STL +2020-12-21 21:27:33,722 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:33,748 INFO ExportHelper.cs: 317 - elbow: Exporting STL with coordinate frame Origin_link3_joint +2020-12-21 21:27:33,748 INFO ExportHelper.cs: 322 - elbow: Reference geometry name +2020-12-21 21:27:33,766 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\elbow.STL +2020-12-21 21:27:33,831 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:33,841 INFO ExportHelper.cs: 317 - upperarm: Exporting STL with coordinate frame Origin_link2_joint2 +2020-12-21 21:27:33,842 INFO ExportHelper.cs: 322 - upperarm: Reference geometry name +2020-12-21 21:27:33,862 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\upperarm.STL +2020-12-21 21:27:33,946 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:33,954 INFO ExportHelper.cs: 317 - gearbox: Exporting STL with coordinate frame Origin_link1_joint2 +2020-12-21 21:27:33,954 INFO ExportHelper.cs: 322 - gearbox: Reference geometry name +2020-12-21 21:27:33,978 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\gearbox.STL +2020-12-21 21:27:34,045 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:34,052 INFO ExportHelper.cs: 317 - turntable: Exporting STL with coordinate frame Origin_global +2020-12-21 21:27:34,052 INFO ExportHelper.cs: 322 - turntable: Reference geometry name +2020-12-21 21:27:34,070 INFO ExportHelper.cs: 330 - Saving STL to C:\Users\ustuner\Desktop\wroboarm_21\meshes\turntable.STL +2020-12-21 21:27:34,126 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2020-12-21 21:27:34,132 INFO ExportHelper.cs: 145 - Showing all components except previously hidden components +2020-12-21 21:27:34,218 INFO ExportHelper.cs: 145 - Resetting STL preferences +2020-12-21 21:27:34,219 INFO ExportHelper.cs: 478 - Returning STL preferences to user preferences +2020-12-21 21:27:34,220 INFO ExportHelper.cs: 228 - Writing URDF file to C:\Users\ustuner\Desktop\wroboarm_21\urdf\wroboarm_21.urdf +2020-12-21 21:27:34,314 INFO CSVImportExport.cs: 32 - Writing CSV file C:\Users\ustuner\Desktop\wroboarm_21\urdf\wroboarm_21.csv +2020-12-21 21:27:34,356 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,357 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,357 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,358 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,358 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,359 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,360 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2020-12-21 21:27:34,368 INFO ExportHelper.cs: 234 - Copying log file +2020-12-21 21:27:34,373 INFO ExportHelper.cs: 439 - Copying C:\Users\ustuner\sw2urdf_logs\sw2urdf.log to C:\Users\ustuner\Desktop\wroboarm_21\export.log diff --git a/src/wroboarm_21/launch/chomp_planning_pipeline.launch.xml b/src/wroboarm_21/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..ca38555e --- /dev/null +++ b/src/wroboarm_21/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/default_warehouse_db.launch b/src/wroboarm_21/launch/default_warehouse_db.launch new file mode 100644 index 00000000..1145f07a --- /dev/null +++ b/src/wroboarm_21/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/demo.launch b/src/wroboarm_21/launch/demo.launch new file mode 100644 index 00000000..54b5b132 --- /dev/null +++ b/src/wroboarm_21/launch/demo.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/demo_gazebo.launch b/src/wroboarm_21/launch/demo_gazebo.launch new file mode 100644 index 00000000..eb59236f --- /dev/null +++ b/src/wroboarm_21/launch/demo_gazebo.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/display.launch b/src/wroboarm_21/launch/display.launch new file mode 100644 index 00000000..5b8e9435 --- /dev/null +++ b/src/wroboarm_21/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + \ No newline at end of file diff --git a/src/wroboarm_21/launch/fake_moveit_controller_manager.launch.xml b/src/wroboarm_21/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..f6c0f465 --- /dev/null +++ b/src/wroboarm_21/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/wroboarm_21/launch/gazebo.launch b/src/wroboarm_21/launch/gazebo.launch new file mode 100644 index 00000000..6e80529e --- /dev/null +++ b/src/wroboarm_21/launch/gazebo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/joystick_control.launch b/src/wroboarm_21/launch/joystick_control.launch new file mode 100644 index 00000000..9411f6e6 --- /dev/null +++ b/src/wroboarm_21/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/move_group.launch b/src/wroboarm_21/launch/move_group.launch new file mode 100644 index 00000000..1910c99f --- /dev/null +++ b/src/wroboarm_21/launch/move_group.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz new file mode 100644 index 00000000..fe94253b --- /dev/null +++ b/src/wroboarm_21/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: turntable + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /turntable + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/src/wroboarm_21/launch/moveit_rviz.launch b/src/wroboarm_21/launch/moveit_rviz.launch new file mode 100644 index 00000000..615b2f12 --- /dev/null +++ b/src/wroboarm_21/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/ompl_planning_pipeline.launch.xml b/src/wroboarm_21/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 00000000..8ae6467c --- /dev/null +++ b/src/wroboarm_21/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/planning_context.launch b/src/wroboarm_21/launch/planning_context.launch new file mode 100644 index 00000000..d57c692d --- /dev/null +++ b/src/wroboarm_21/launch/planning_context.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/planning_pipeline.launch.xml b/src/wroboarm_21/launch/planning_pipeline.launch.xml new file mode 100644 index 00000000..c64f6d61 --- /dev/null +++ b/src/wroboarm_21/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/wroboarm_21/launch/ros_controllers.launch b/src/wroboarm_21/launch/ros_controllers.launch new file mode 100644 index 00000000..f23ccb91 --- /dev/null +++ b/src/wroboarm_21/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/src/wroboarm_21/launch/run_benchmark_ompl.launch b/src/wroboarm_21/launch/run_benchmark_ompl.launch new file mode 100644 index 00000000..f4cc195a --- /dev/null +++ b/src/wroboarm_21/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/sensor_manager.launch.xml b/src/wroboarm_21/launch/sensor_manager.launch.xml new file mode 100644 index 00000000..31fd01af --- /dev/null +++ b/src/wroboarm_21/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/setup_assistant.launch b/src/wroboarm_21/launch/setup_assistant.launch new file mode 100644 index 00000000..05acb517 --- /dev/null +++ b/src/wroboarm_21/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/trajectory_execution.launch.xml b/src/wroboarm_21/launch/trajectory_execution.launch.xml new file mode 100644 index 00000000..26c4b149 --- /dev/null +++ b/src/wroboarm_21/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/warehouse.launch b/src/wroboarm_21/launch/warehouse.launch new file mode 100644 index 00000000..a19014e8 --- /dev/null +++ b/src/wroboarm_21/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/warehouse_settings.launch.xml b/src/wroboarm_21/launch/warehouse_settings.launch.xml new file mode 100644 index 00000000..e473b083 --- /dev/null +++ b/src/wroboarm_21/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/launch/wroboarm_21_moveit_controller_manager.launch.xml b/src/wroboarm_21/launch/wroboarm_21_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..cd0d7e3d --- /dev/null +++ b/src/wroboarm_21/launch/wroboarm_21_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/wroboarm_21/launch/wroboarm_21_moveit_sensor_manager.launch.xml b/src/wroboarm_21/launch/wroboarm_21_moveit_sensor_manager.launch.xml new file mode 100644 index 00000000..5d02698d --- /dev/null +++ b/src/wroboarm_21/launch/wroboarm_21_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/src/wroboarm_21/meshes/elbow.STL b/src/wroboarm_21/meshes/elbow.STL new file mode 100644 index 0000000000000000000000000000000000000000..5b1cbba57c6643476f46e02ded4e495662aa8126 GIT binary patch literal 8084 zcmb`LUx-yz6vhuqqmh_3+8=WqYi>|bHsz!?bLJjRGZMo;A!I29GSGuqJ*b2tD%24E z=_PtnW?%-)Ae^mAXFSEF7{r0!l z+H3E#&xxY{_pjd$wD}NSwky|8Jg%35E!X(b>Tsj5^w#!s`g6njFGYi z#{$79YJ}HA^H2iz`ra8`VGXW1qc}rUy-b+7qy9EqQw6LVvbS9l&KzCB%$30UqeIIY zN-&C`#7B7rDsWPMd4eDBscE-PaGyItFmKu(AP;Va@gF-OpYD0Uxg%=Xr^Qi6RP zt`+lO6g1ciWr7|=DPi=+cWo{YB^U+TOLWT$9} z@VgdkZpQ@yJKMDr5{P0>*(sVMFn51`us%3f?0VdKR=#eQK&8Vw3hTi<7zGXPg7SLM zgD55Z`Um#85{&Y+-{-iGbgbxVF}IEMaNEPRQ>`ubY4FfL@6!POpubF5&H$UI8$e4LojDD8!O9$FLbTA%rCtcMajCBn1f3ThN8!r$|=QjZcm z$ATK!SQ>@3!E?7vsD~$~&paEc@vM~K8C=%TJTwZwH(c|~qXcX`al#rLOQW=>?dem& zodlYbw_P`Vb#{1@)lfog+q>?2Ns&M=`7e;!xX|P8QM7#O zlLwW7!8VWW_TsT;munmmdGJp}@F^Mu(^gJ#VWt~os*#6~Zm8m6y`1U&@; z>+cD(2M5g_OxU~hfDju!?Z2=T33>_y_J=2mw?2A6h;23aww=56Wu5|IX-AkHW%J-! zf^qpyF1D?eJFkiavgP}^5Vi`ASUY_0pg9v0o{36`%{7mss7Ux#i&-n`@%JeD_F8?w zob6Sf?eu^U+xD*e22do>iSQH%+q;g4r%oO)&!{S&QA&u-J!tYM5`Gt14I#L@3^8wT zXL|OrxPD;6tbX2D@#Lh9rzLO8?pdpZdP0PI*8X^OL;C#brnG(Oq<$r6BZ~KVr=f&; zLWI+JVa4q9+{#ty=p!S2O3+3WZgXy?dk06?R`qnMig(dPD2Ux zgb1gxIo4}18=9eUE)1m3EI>c|N2oR=m`@^dP0QL@LiHjT5!OwImaT6D0IyD*Fh!J6C#|3@Al-x^xnBj z&_)zGpI?p4+PNKN3H5{sr{Q;P^5FK_eM-6WB^--1 zqHq$8e~nT?Jt4wr_)|2w{^~#WOypRk5rq?#-?E}e3H5{sr{SLl)uk(D+OwTwkwz4s O3~mo9p`H-oH2wu$m;KNH literal 0 HcmV?d00001 diff --git a/src/wroboarm_21/meshes/forearm.STL b/src/wroboarm_21/meshes/forearm.STL new file mode 100644 index 0000000000000000000000000000000000000000..63094c85c87ccbc3df1eeb137add10c22e680d53 GIT binary patch literal 14084 zcmb`NU5K4m6^3`Eibxx*tt8m0ZF6R{SlSTUi#mrnf>1)P3Y8$eP>ZP-dSgIn#b~EL zsV&5oP%j$%0h2S5l3J`JBALUS7SubTcNz@QN{|t}QBsVyrLK2>>)qeGf4(zb7zkO} z>wWgxf8W~cJLiP(|NXP;su2GC{cp~XT=??l?Z5u*=*8>r8;9`8>FMk5+wAOcV((|( z`Nd<8tc?Hp+?f@$=Y)D%1n3ZMTHZH*-;X{%|I<&Gmz1Cl)gLyl8XtK1)^Qg@3H7vy zE=E|sxcS%hugqWj+rA|wXhZe#UH7cqa$P(nQ|!efLNMj@=tk3RU>B_(Kkj17sA z6Uoye(mYnI%()dc72-M`#>QVH5n*@>Ci>4@hoo~uNtrzX~J$zrV9*_|rU*`3PijdXSqBNFOq z5nT+Q`NYodx)QXZ@>Q6nvy0V@gnC*;7sGdOVrO?<3EEKkZh8zQ)YBq7MtEUrXLn*} zcU=kE9%IGME@R|G^0bIF4?KgD*Oqr8h7!?+%0`IioH4qmHWKO~twa~YGsDEr?o?O=-+k$! z5ppihua9?zzVsy8ZXzef_MA{vL8nt$5y_(j=79fq^UzW0c~zC3u{?&3*G>s??FRbQ zcsZe-oIn;``$;`2B|O7LmACtT=seV;gvVQ{W2ndH6#Lbln)8S%C48T@8I`ImMmjN7 zjO0d80(UH=DxR4WvdV?B;Uc*)9|Qr0=a~lq1cbqSC>~hmCt-uk4imC zc)WPVnxl$+smkZ)F)TBr+Q;{H=J{%d|ErxcLn1t9L={IU*^ak?#313h1gntI-OP1V zEdnZQk0Wf3DtS06-@(}4jG;tQVRRfJa{Jl@BeR{fy5#kE-`)0+r;vBYmwxb#*(=A6 zMjOxkj;-%nO|a$R5|hU>5*%n+lC0CQ-QY8 zuC(WYjQsnzzZGX|%6OJQpvrjT_*x?dJy0nD8z-h-tCE0^aJuii` z>lYqAqlBu^j=RC9dk5vb#(qKYj74IO zdKCAed1ex0#(vQbDvc^5=z&U!5XSv!A0(*q7_)W^o;axFT*Y`PCuT9^>}CW{em!#~ zs6xCDKHul|ECxML<-{-h+&)Nz(5XOM?Ea1_h+bte;=Qz(U|&9}MA*8;JFz+Q7###v zux-`*yqk@Ro|0fjtz#%b6=>Wm={e70&;ykc%=2{&C8%PI^qf~Q;DJgB{6F1;g9KIh z7xPQcc}CC!l@hQAPdo4PNI(TOb~ZidSqyp@g9M+hdaZ0!F)um6%z*tE53W`8l!X0% zJh)aF;T1rPF}UZ<)G_0bU=FV5f$@5kfgOxD07i)Ur8_EHD|(<(f_F(BLkX%tGZV#1 ze~Qn1>{ggtbsu*Mm9SRD-J2diheOp$LOn{jJ??7iZHTDy*!~%^66(=@`EFRp(0RBj zzCq{+^O>th34b^A_YWPF5~{NHF;=)9m3ox$ch2pJ_?ZLx<(pR>!)89VCn~O@#~VY{ zz3FwI+~+e_j}kiCZHYK`kL{nT=~}5r`~BbM;i@U}L* zjfyKu6=+-aMrCQ9vFzYj_6E0ezvpwlL4u$Mwh|%S(0Cs#397ukpLrGoqe68Dl{~dE zUdo9Xfsy!akP*x)@mviOR3Tmn`x@_q>VcgTC;A&Fi^0A+x>Lc%ezfPI`=Kk%=f9o@ zJ?K{n*md0wRY^d__PTCY4`YyECTfogeSx3~Hg~or=qU+i)WrlJm{a|(29*+2q1|?{@mYp?U@I~FM6oXrRKd0a{a2&++V4v~YQHnlzEp*F zTZNb_Yl0pnV6#1h$IhM`U9#W%e87JFqJ60f?RHAyTX;>-qXcZW+zhhvpco5n%LRpJQ6_~F>G}`iqJ*q+s>`hmf*xAhpdX#|8_7JZ8=SF0>Rpvx`RD~GKl_7lU^k_*5dX#|8 zj2gn$b*H9F=MPT~?75lucC2yA?^sxI3=`;=DvXMD2z%Eyrtj=JIC=cgR*XT960liu zEGDQz4B8>Q@%(K1=GAvk{&Dr)@%f-f`-RFsghWPCRt($&d}5Z zUZcJ$38|bgk4Amv^I#R0REWWvCU{_sbFPA$GX zP0$OGmVQL5==J$GpNP1he(r(G(*!LfAa72#YDX{RZizWrws}nMU8bIgT*hj5a;&O_ zL>l#t2#xyCs!Grc{kvj5NWAps=Ec&gF-M(Sda)MTR&h))=UcR@n?YJgAjh`NL4saM z?-A*8(CeQczv14hRVN5qNTf)fkFixX7QHZ=9YG5!X2aY=&ub16sVbw!QssA*$XN7( zeykGbIFNumuG%%?5-kiO_QP2{QuYtM_0sNhKR=88>ifTWWq0TGN4R&z4CWow#NR&i z&E41k^%`gGphzG6#CLbMe)Q`Y-IlP0y_M|!SdrUSk+7GD=O6w-`Sj0CGhqo6rJYY!t)`f-pK z361p4pT5=3AbTE!y^v$29~>(Zs)g_U?mcfq*h@KFJ+&XRBw4tMzVXJ7YmIr-D?N8jD!P#VnUA>1#C({}`inSu?5*D>vW0Dz z>{jR7FrA`D{P?AQkf>f+e{UypjmQ?pmF!W-v7JX2>?PV2svQ2A&#^DXh!(nhrN*8Z^U)L8 zg0=_keI>l-%s9tF*em5073aF+>vs^SxbM;QK@0 z>dL#-_vbW`prt32BNJbH-(yiFm*4upISG8d>~+)6{nHmc30it0kAjGwedkM&Ga?^c(pGF-xMQCrXY)lzz;qA9qg>8}zRu zXhDu3%2*|$jP9J~<46KM-(a57?Ue*AJ)sZ<1;=bdX zzG#0ZjD%y!%eTJkw~B;s&SQ=;Q+oN28>{Z6$PqW0Wku1_6TUe&IpS{`0J(SkWgCH5_-!(j@7yf zBId(!MffZ1WR4*0CBo~bA!0r}Ch}5^=_M_al2GN3Z+(m<*vIaE= z4dHpZny{B>SN=$#+IJow5oNE;bKZ?r9o>fTNRK%jOL6Tb`Sj?@OE2{Da=oHed3~(4 zO4Z&FuKbg&3c_B<@isjt;q`bjN6etTl*83iiCK~?-jQ=l>RA%6we=-cJ+B0IQP)!R z$pRNn zR?#cvsHJ}X3qyLX$He9Rrw_&oo*G0G#^3xEkjhy zN32>`BE67ftSk3i|DU18g*?{P8qvLFAmk}dh25>dP(ndcO+~f zA=&y&r1cRu4aTCE_5Rz0sFv{G=pEPeLZ}}fOYbF&MK9^qbN%0jr5`42AtBlNOqBUJ zG8Vn0*9`irXqrgaLPE0jIY+6h$YBe;ltY#1ztK+<30p`=wm#=5)gC!)p_g*-W;snH zY#|}p`kLcRZ+(nKFX?@U9|>DXNVYx`|4#3He@<_Gj72Z$eTN?jTS!Q@J`>NT_r864 z>tif>NzXgnG?B1{gkV21l}^jiPR zd0uvg7+qT!SF-iFA3g^ei(b-eA6xDvBy1re+4`Krry66?OM0JOBVh{($<}AW=OAOz zOZw(ii@z}G9HfPWgY)wreV3W9_*Y|mr#vaB3HiAL-kUJ^@=sjMpjv)OOS>UlojTH_0g}Z7Z>S9z=T9iEJ}XYMr+r7BCmU5eQj|6>!~d?P#r$VgozN&^YSg-y$-rJl&Z!Ahz^6SVQWkU6wI(RLP()t{NJvwKbN7gQT z#TrcLj@hD)GHTu~;Ympy6)2rYo@^(p>?w(1HTZbmpGPKe<}6C;!}tEovxf=Qi#x{6 zDz3#*T3<_yaj)ZE)b7T+&pMrMyJ7w9#?b@mrp66TM9c~v-R+S!jV{3+%PMK4%DaU1 z1fG%@R*R2cZjlvwWl_44J`Kt`+6qM@o^9=n(8-B7`bF zIp+`$I>xv%U0Q{_K>mo<-UI?N8`DSxzY5(%EW8X*R*&bGp`j_Kf%bhL> z_JE!x(vx%B?kml;tKXNuEbVx~gxQGV*@VCZdpsg({Mvq`kX-DkpVM3wVrf;}FQG-MW2naRqTr}z3yn4O~99TNf*GdlzKhd#sk z>5O?p3qYnhEG=>p!)b?;(_7{MNoNE( zB-rB-Nkb>t$T=S|VK$<4npH9LNU+Bvl7{ZYM$Y+|39}KUyHYinV2?*64Lwnfobxdg zW~V5A%V1*WJprdn-*Maj4fD;=$T^qyQC_Q!61@TF)GO=|nR_HkPnXEO!kyU2IY%9N zEwd3NU5NivCk^RUCfMTzub5zuM_I4-sj!hH7N`PhMoEBcjw3M-3x zTG@Vexfi2kUHg&GJb6fn-cp>fn+Nw|l;o%V&zy=LpBSqZcPeX;kbYFs^L#&29*lC0 z{9L6OCE@mdwz|`VMfnLcglBHk?zJ-eY^Nge;cop-PGoepR+b^7MB_mJvw0*B3DMK3 z%xffqQ9jdMVkB=#>ly0AHJGc$Pi5;Vk04Z|N4Q?4JCzAW`KdAlJ-Df7JQWF<5`BGi z68~{3MtSDik33->5^n2UolaPkpD;s6ee^zSry5M9X1=T^>udN_TZT?pMV+ud_4=WE znTLdHXZpdt7{!U{YA{3ZrsbqL=zl*CdPw-h7~Rz%!6;cT)QTPw*Ehb!t?hR(?Zqgs zLgrLnQS*3jtk2Z$<&iW<$Xw|wnwb*r#VB7trROBvw&}^9%JyOu=S_*t+PMMLJKrp@ zZq>~aoN&_757n998OdHcKYFT&3Om-zL}&z|Rk!>sj~038UI(rb`>I~7kMqb8sxeI@ z4cC!xWvV=-5qhyc$)lCbNrXlaIMt7%g~Laep0&s4wucLM>h%2scH^^KX?}wN33>{I zoa*G{nH8%<@blbuYJBk0(OJR7Y!2h6bRAK&)AqN60k!<@409=v8k3vnWsQZ@>r2Pb~A*% zAAUmD@H~v52Xs`|=v3WW`Y|E_J9@4!pXA~FFv5Fao&q6x_>|N#S0fUzV|HDGgwK+B z3WR9*wAXS!MkHW2zvkY&l^k4y1U&^}tg#|@&@@QE);#bXPBaohPk|WMYDMm#)rtgc z?T7vzoYX22^b`oukPg;z2cv^&FVH$Avj6y(WtX6*K!}D+Vl8(tCNb>=TJHybL6V@S zK!^rEqe#FGjiq!4)2Et8nWsQZ^02ogLns}!at+VJ2tPYDrin>E>`f#AJ9=KFJE$Er w!h2wz0wEf5qjXB_Z6^UcX4f@H_$-;HK!}D zf}*>=RacN1Mi4DU-OR#;h~URU!M*XGI8Wpo8JV{bEa>jjan5tz_|C|Tnx_Bn&!Z33 ze7}D8+41g$H_q=oxj%jF7-n#wGNADm1dieS2GC`CwU{~UguROkf>4Vop z1CKP^{`RZCI{x!BZ>7c_L6kJGtH#S8yng-rS00B39%;CJ^@-mcU;EOhQe%%GN*dTz zJ50eep6ulrmv=M2rI-&ue;z%S6`dAPwwFWXrGJ<9TIg zzoo_=L6kCKSB)IO(7@w)W#{;(#vVbGG_b2i&fU0f+%TVhsONxZ@qc< zw=aJq=2i9F9G1K{(Yv=_dGGkSFTPYYIs%UqvMH|`|M~FC=YN$yQpe*_D*x!u-q)OX zIudxCkWG0d{!!ZSr_vVB*vcAuq zFuf$QuVT!dz?Me#;i+mkfyeX8ro0k4=AnVdqok2@AvK)9RL+_oHSj3P37*uAm}Pk` z*+j`^A8Uk+An>e-(zERZ(AqJ3F4;uM&f3OI?0c7yQdEvX^e!Vi)(X!yakKPppo(&$ z+Bc!`teliM_4T#Fv(T0JQJGWhSB;34J3*A_zn2pqr@lr+@8VIE6Q3{p;r85Rj*7^B z)0)3xPLF+sXHC5GWe_mV|p7Q5uChmicFy*VodJ(DocMEjPb=>@8-3*jQKEnFM7zTL{jpYf z6y?MZ%QK}!KR^&AdVkg8QIr#@C!N)pJ3*9ac8o^MvgFAfE_&2X1FVmYzVf;21W)az zA;Z?keP|FR?Oaj4#w?HTKCY2_@IuUJ;1N9%_X{B)G@S5WV7^5lXNDl~toto@@LnTY z?gUYynU%FN`ddH6(@uA?vxXc5iSE62pRhIG zy+`$gt?iaqdaVDkR!$Hlg0sX(;PJe&yS;?I(g?O(9wj@q>!}YSde>$~*V8;j$6Doh z#Hi&A4?BDMG!l4J6DM+<9w3Mkonx~>;8Bzl`z?2ZDA74K8ya{N<;3QAMDH@IMcDJa zt%9+yY-i{Q&JT^4Wht){M5&xzVkGb=%8C6kcY-L<>~^CD9z{99YHoy#XonL-iOv~w z7zsQ}RP4cw{|4bTu`^@tM7fLZu8)z3v*NP*%&Lj6sNCW7WVmI29NU3sq3d@hcGr#w z4JU{ay}$3`QIr#$44TF)%c5d!^L$k~&)`PL2m+7QBxs)0jhMFxqC|807zsRza)R?i zBVOy3Iw$J7MBQ)@+CT{yInWjC0C~06< zjq-*5T<$+Y1CKP^W>#J%h>`|&CCdHBx!ivo-6IX6WV2Q-6GSN!b`W!F+_}f|%1*r) z^J&^6h*Bo(s*!zlc8}+ko&A;?djwI+gk3dq1VaOl=arq~pBj4vQPRM!8aa1E1CQsG z%~}b9***|NNdvnQ_3v2(XG48|;g%%qH$V2B`2O1yCF)@N)fKkN_X#|z3B*vVxhR!C zv%ZVATppzwlBv}SqIsMscHdVeT0f!TM2iwZ4Qp)PE=BL+@m`S4IruU`lxm1wi84## zF4aArS2m~G%LGx%gk6ce?TdEc@w~D*2VW+Lk_L7qxJz}9=apUlv@fZ#&g1rVO%SDW z>`o*0D?H`jmREMx>TsDL%CwWnzPfgg=arp(_y9qaGGW(R<(P*C9?vT~XGv=85kyG? zyJ~PV>K@N4JLhz2>=8sM6L!_8f6vhlJf2r}uG-XqC?0m4csd)++T0+>RcgXY?VjzC zeRd5eI00O&RjaQwAk=c~LBoj_HS|@xMP58{*PLTeZ)Nz#Q8l=!7J+B!-AYqliM$z& zb&f{_wld{AZk&*(*$F&O$fmq%l$*G?clKN!rEic6%InOp_c+t!1Rf`3Q(iT=MfO}C zrE=aAMgor$vMFyZk6UEV{m{h&3K$@t+KCThsz#~FB3*E9!cbw z4}{Uog$*KSMrasuOL?tT+2M}m_pd?&k6LbayB{72{{~=0_nl$lH#tJX3A4K!te)^M zUPg4^$l{3;zbB)yb;9iKCax#^`;`&hH?l^=e&vMOXm9LSnO8eb%PX7mHcQ%mg~y}R zzU+m%YU$mxdsYPFkwlL9K#*6Nax^yxJkl_N856{EUt4yv_Pcem>&fEn7iE_}<^ALB z_`Ri`z_TX&9j7PotO?E!>fL>UDAD-_a2g3bigLnF8NG%RM2Yry&Yr-dC@1REAnt`% z?ZC55)RRopm=)zjK4pgQ42_r;;wGZ<3HJcO*8$N^_$i~;aDph&yfcjBfk#nJ)U(8D zUPa6C81eLf)(C> zy+l?F#v9-PmnzcDQU2er^FTe9Y@%dmZR6bD&s{ugBK7)N664_nQKDHZs*HYOmy3C2 XD-Qa|&Xa literal 0 HcmV?d00001 diff --git a/src/wroboarm_21/meshes/wrist.STL b/src/wroboarm_21/meshes/wrist.STL new file mode 100644 index 0000000000000000000000000000000000000000..76da8c3574e5acb60d4e9171c059b2cc0e1944dc GIT binary patch literal 21884 zcmb_kf7E7GRX(t=Kn)sZC|zhVGv8}bfGCAH-<|g&EiJHg8Ye{=SHvpB1{j0kcMTL; zG%g1bmKj1znC}%yDSkMAG~fN+D_F+Da-pqQh6S`-Ab*HfET$X@+RwiGIrH2f?|l3b zYklip-m{;x_t|@&{hV{&^G-_tzyE#lpp;g;^Ro3@ue-Xu_DvVA{oLfiJ->4OJ1Tpw zJLs{mueo#Oh6)w@vmS9{O> z?mXUK*mqd9r9Qf_zxQss1$V23n~55&R;KfUA?)y;bjNA4z?&_>+jsZ zc-;7u%APjz`I}db-g?mo%g?O-M$5Y@rG884_UA4ieP!qF@tfbj)pdlVk2btoglFO5 zBBdL*eWkentDhdPdhp^}Tf#a-(b4QFr5ldDbYvEd&9asbMZ_2|7Sr+W51lu<=lm7r zk$=6hg-|JtW=ijQ$8E*>T|ZK8yz9M=V4M{Z9!E?}$JKM^j4XPkMX`lYDb0nHHe9l$ z*!0hPm$n_f-4Vz(_=Fj0PakZu zt#9pJf83fz2N5cTkux?az3q})Nd($XX}Z@b-%l!{?E@hS%O)Lxy_k{IKx+%8%n$p!V@m1H>hNO6EoJ?{mvZ$fP<@z@uo70$+LZq0 z`ol)g9`cJd{OOlzx!bAa`rYU2jn-9aBnA{OwcoU;F$y_1)iVst+QDy31J# zPe3<(25VFL`=9vi9Q*`-X^SGF5(VPPH$OH9i=-xci%RL3zz!Q0Qo5%8g}2S$3LVH# z@I`P2-q*?J>r#5>9gp0O3_+%7iy}A!kc}x_@JCDYH|@NRm&;`}lexygL>tw~Db5FPK%q&b6`gSX*>V?Qs}8 zPmG;wW9Jf5k#UQ7k2z6E9#@&2Yh&k9TO^k%?aA4B5}DZ2p$N%J8;7y;#MrqucJ_GC zDwTqc$Pz;YJJ-g}u7fR#keD07*m)9J;yQ>>skX0-ohQc5Q)6e>VUnmmK3elO>SHDK z(R>x`JT-PMwZ(M#Itx#XgFQJrPol!LbSOfqo#`-ko)|mV#?CE-N@+ARJJ-g}j$oV> zA@$017&}j*!nF`8rMZyVdA+f-BO)8QlJvDHtTIN8wdoztUNqqfR$GE)9MQ073dG)n zubLP;FBv<#4)&!|=!vn(?7U>`OshO^wGhc0}Y8SCYQ2 z4yzV{batLtbsJliqYiJ0m^&hL1j5)ER$1 zPO$EQzbJL*%7?LH@=V7&da@_}iZ()96u}%jFhZp`SNaH68MK7H0`E=j!XBqZf2 zdDz#XEv7Fc%)$b>)kj!f&0AJO(pTc9&));>?`^BJB`kEQ(7g;orNS!rbX$kESTqaN z6j-Rb+X!t@gk_J-i)(J{k6aNd6`xl#5#FMR&^O>+?MtO*$3tTtv2lIc)8NQmN)cA= ze7$hsaj2B(Krh}?Pe*Q11S{&G4n@RD>0#`7eC^Oj=sXCi0#ad>zYkjFEs6-MmO-dg zcy}1#pTZJF*gL$tJR(a)YyyFrj@_5l_z|>|SMg2`534HEZ7YBSAs~T2Ajlzo1ZEMk zFr@sHB6Q5K3Zuqf7h%@MIYZYfMNkT8)D`4*-zps~`8BMXF>;I)e~?lHSC@SqilCI} zunN%~hf2jecEa!mbPpid0x3m=g##m31f_ruj|?C*g0KRAK&12;<62a@?l{;Iwr2!& zFN^Te0v#)x0UeAjq--3R<2WgDU9hmPLv2@t`3*=EUGFkdkct`X@tEyhwsa9OYlnT6V?ZgOXU8Kviv0lRoxgK@{c*6Ri-;M# zjCUyov{~rq4}BeM3ENSL+|$(EN3f-fuqYUxH{Tu2jv%Fgww~a_;c<8iq!ba}T?RoZ zpd&VhKuyQ~#`>#eh143Ytb;lTZ3)}mepwTB5Y|#bJ4R?|-&)F-GRHV555+EW{$EsAJr2)v?{kK!g7o|$WC5qUn*r*d4=oi&=wnq^Ae=Ar}j9sMG@w=qN%|H@yO#) zsp!eu26Y5#f4`w`U`A?kbX+62``r}|r7hx7UHy#*4DWLcBc{d(myYp*|oLmb}TO_-P7Gt-R7K z2}?XnVtm8i^=OIhj)*ZWgP@f7t=WIHtcn<5b&t$PZ2bMDS)~ZDGZ|Jk#lp^B2S?RaN!M+6HXUNRAzE#WyAR;!5x$EvDREHvBZZ0zpO2xFco3<#A9-MqbQBc!nz?-iPxFIuwB& zJ;&tPzCpifaB28mEWtZJcdG^wu0zKZ?+v1Lp8@@c4)n`+e*%S(w{Jj%6m%$p?}pHO zGV}+YWHmg2j`$mbH<0X>-Y%}&@a)_o@Utwe(%h<+udIKc&*(SaaYw_~*A3#8Z#;BY z-0Q>l9|h+cS2dAW8SOU*s<_wZ2==Q8=CD1jigv#F$$hC*+p0rX|6n0*0^&O_p1mN$ zxuXYfc= zRon#hanQSp;2cLS>wO=Uu3@(Oni^1I(PsNyCd z?i%D4Rs~oIG<)iOUz=ML!4)TTr*y*R<8jvjHvwB#sT6bs!x_SE0#m~Xjc8h6&K>J z0d4}e5Gtjy@w*09+ytam9yLXTKJ3>_N9SAp96^=R$ogG_DsBS04(3xt5Z^X2WBd## zrMZ8(5_b)76OdMUiy}ftAnbmk-hhBr-l9_KH@|C8#Z5rh!MPU^^zYds6XX2q$G&|5 z?i%1GU~3$Th>Qt@-D1^yGRR$TQ7Ijh-!-V>CZOwJK81CjXOX)&Z?||xCzKg?W`Z;@i_^rXl+WnUvbmFQ-dwsZdpQR{`r}pGr zt2a!u4n;_gSv-tu=i*+UN03&j6m)RrX0FA(J|72L6d_q+Iy$#Mb6+Z@x!_!jTlcuv z=Q`*~sc=4W_K>+2_xf<_o;5M+f|VG@enjWya&F;x@RZtR@i4B%t$W<-bF0{wN@+AR z*WzBEBNzuo(BpXaZZ7ATtCZ$KW;NWPt2zJhgeCM_FdV)&Fdf&O`*XNMk6U`RwkQJc z{PxVo@s}UNJ$?L^rH7zY==PJ&yaK>&Lt5o6+HdGvM7-L!?zJzKipU1Sm=kyCaZ8Ue z_ZCG&H1Vyw>9BkHU}xL}thGfEc(=%6ZD$DmZAKSCsfcDEj5%?K9=G&rZP9)snz+Yg zI_#c4*cmqgYi&^k-Yt84HQxShMi)V;$dW)9bK(v?Zt1a>c#HNM*;(P4*%>zhYi&^k z-c5gnx!?ZHPZvR{s1kuN<{ZT>J$_f@E!uC)vI@`4&bSFU)fPqI-D2hEGwt8}bP<$_ z>JtcK&bheVMvn3p?Kfstg=c1G+@QlvKwpjFsR+DVURPj<-rd0#f>OaHfxx+|t&08b zptfkg^0{#o*VA?*p8HzwOQrOeXem8?(?=FC$1&GK3f#Bn?Z5XpZunYOwC>ZJ4*X6H zD}WzAHE)zx}!=7ql_J4-c>2~ zWKXjSj0xua?T?&0AJm3IktV{ z?3Rupgdt4_7!%BS*HdS-5bPIHj8o&Q9dCJG{h9|}R^Pns)Eq&yC?YDK?_N^kFKbR* zdD=w#QYn6pTuAA%W46~PJ-T=OubuD==AUO9bO`&{Nfff4-;!}|Z2rkhC)zTMz%CUW z3$DFvx~+xSkKWLVjop~ves%p*|8(Wj_LpzT@qkr|2z{74rXzllGX1w}E}CeZRf?ZC zF@NZ+t@Yg>+dcl_rYEpR^K65Th#=UKR>gOB+(~@=>@C#cEklS#M_4uf^ZiyiA}j$@ zw>+KFVSlx;-nV5%`P9CL<|9%z>W8a{AlQ=e539KAdi;kQnOEL2gg}nv+#Nsq^_;to zpcf#eV@m1E_a9q-;nuReZuhgF zFF;DiWHJB#Gt)o&!a3#5w|<&CEJp~#5UyB$! zw{)QYE&@nr=P}NZjh*MTFTUf+aWuX%b}o&b%V6gmHAPs=vC79<)O7HdHO9_!+Luc4 zb3C1$OPnDaJCE28w4)Zn+xp&AQDf)Q*m)f6oTG)DV&B@6S2+Kbg|Tx>#}EQYXXg@U z$i~hs1pCF4UT%D4>|7cvI;=aKfMQv4iGXXnz`c`4W#BcF$M zWT9oFXM8hf{FfGnh}pDs#3!yDX-gmBcMS}IvtyhgKk~(mEkuKEq%=0p&ZV*Q`e5fA zHARFzpD)hN_=V8e*mLa{*|{`!u7jNs^8!{y1br12@eixGvom&PUU|z90?3@ZQ)A~A zg8kyDV{&#bjh*XYXH*}zDk?)E6~aGvcJ^Jcu`@GqCZfs2_*GbK?A$^`z7}}un4F!* z#?E!HGir(Jh|1vWt(Y5O6;BL|omsDDB7n>_u{L&QO>_kN#Z$eIxwbYoSio$u7#FBI zF`Im+$M$k7k!gqV>Z_yi$$|tFmLSK-ye4-A2Z*&Al)wTj4NBKAu0V`>(uK>oj z0;ppJko!`psEy%Wo)Xy#fR)-?h7e7qj@n+^3ZR8xzj$gieeX;@Uy6}?o$7GAR|T~D z3V^$9An2cUo!$1j@S7pS(gWcH7`{?6&iI6FP(kEk0NS^Ouuc9UtB1^8p0!4hEkueXh;t(QhNAvB`Xn z-FA)#tP(z_KA(q~&!-*cY)Mf^6Z7D6?6&jj5jrA*K63-Hh|e>hk6+x+=iV}eXms!w zznxVEv(KxidOGttcH4^)sS4U7g3hE7|FA0fyu9uC%;(-Rgg}nv+#Pq;qRcBlG1oC= zKF4kw`&aHJSQ#qR616t-d0GBO=5rrMa9<12WMbrQ*;$L`ej%k}%6yL9w(c5O8GN>? zwZ7Z--|4^iRhiG-s;D?E1Zr2ViDhSx!^-LB;yR|xgV+hPw!4m~=U#!LCi>?o1rL_b z-kW)FCZefuGxj)S98^~R8#z9vd?H*LN9m3p2w%A($N1jph!bJ#IpJNG!qdiq+UVl| zV`7z_v0IQiE0?6LpIrqhW@w7kiW9aCdx96521U~ci)fn?ociXU+{X$A(lTWrw z<653NyLMzCVhuFko7|0)ZS0*$!HmjM)PZL{*&Z9$PEcJilb{3j4_?*x zrY6S8Hulb_?Jk9L^KsHPPR+q+Ll%97f;5i@l~8`WAB_J zh!#ay^?~oa2IrIQvU56!8kMD}qsfvu*~Z@4D?jE1bXayH8OlG4bg6eru*J3G4| zpP(aRj@m-2{B!48|Cg+cYk9Ig6Vc>VSk*mm&;3G5J)KXsv3Ca7`rLz65kbsW#y_lz zlkM8LmU-nZLx?65XM8J@`-PNxA)jnx@65Aei^apN46P?V&nMfpaV<}_XCj(RjFauU z^Q{c$gg + + wroboarm_21 + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the wroboarm_21 with the MoveIt Motion Planning Framework + + me + me + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + tf2_ros + xacro + + + + + + diff --git a/src/wroboarm_21/urdf/wroboarm_21.csv b/src/wroboarm_21/urdf/wroboarm_21.csv new file mode 100644 index 00000000..9eaac100 --- /dev/null +++ b/src/wroboarm_21/urdf/wroboarm_21.csv @@ -0,0 +1,8 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +turntable,-2.82541936962439E-17,6.93889390390723E-18,-4.24870678161208E-34,0,0,0,0.396806230030924,0.000571454150237484,-2.23834839552239E-21,1.37054783361242E-37,0.000571454150237484,3.23777898172651E-20,0.00110024104924718,0,0,0,0,0,0,package://wroboarm_21/meshes/turntable.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/turntable.STL,,TurnTable-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +gearbox,-5.70181409604587E-18,0.0380153243966357,-0.00138922050969218,0,0,0,0.341209853046385,0.000637264883384741,6.24923664103596E-20,-2.71681384497167E-19,0.000834504397618049,1.62941368108056E-05,0.000889318878925749,0,0,0,0,0,0,package://wroboarm_21/meshes/gearbox.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/gearbox.STL,,Shoulder-1,Origin_link1_joint2,Axis_link1_joint2,link1_joint,continuous,0,0,0.0127,1.5708,0,0,turntable,0,1,0,,,,,,,,,,,, +upperarm,1.49614561175028E-17,0.157206632847804,2.77555756156289E-17,0,0,0,1.26412421587631,0.0136779903910557,3.22448390771324E-19,3.96119838307109E-20,0.000964387075304149,-2.67429553724681E-18,0.0140917496737089,0,0,0,0,0,0,package://wroboarm_21/meshes/upperarm.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/upperarm.STL,,UpperArm-1,Origin_link2_joint2,Axis_link2_joint2,link2_joint,continuous,0,0.07239,0.03683,0,0,0,gearbox,1,0,0,,,,,,,,,,,, +elbow,7.33210423758527E-18,0.0462797200039115,-0.00991708285798099,0,0,0,0.625328404239412,0.00150936287881219,7.22519589370286E-20,2.65311197270661E-20,0.000771504125821615,-1.13468871077225E-05,0.00102329905512323,0,0,0,0,0,0,package://wroboarm_21/meshes/elbow.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/elbow.STL,,Elbow-1,Origin_link3_joint,Axis_link3_joint,link3_joint,continuous,0,0.381,0,-1.5708,0,0,upperarm,1,0,0,,,,,,,,,,,, +forearm,-1.32860243738577E-17,-1.90819582357449E-17,-0.113066392198771,0,0,0,0.460632401231927,0.00223620566444836,2.86239968572403E-21,-5.06454254732662E-20,0.00224684031586386,-3.05817065058502E-19,0.000245602268076572,0,0,0,0,0,0,package://wroboarm_21/meshes/forearm.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/forearm.STL,,Forearm-1,Origin_link4_joint,Axis_link4_joint,link4_joint,continuous,0,0.04445,0.04445,3.1416,0,0,elbow,0,0,1,,,,,,,,,,,, +wrist,-7.1276563552947E-09,7.80625564189563E-18,-3.41498918032812E-09,0,0,0,0.0642008901362293,3.49809093099438E-05,5.28878027298075E-11,1.56594471999825E-18,2.42909279281571E-05,-4.37083043679572E-11,2.86709715099874E-05,0,0,0,0,0,0,package://wroboarm_21/meshes/wrist.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/wrist.STL,,Wrist-1,Origin_link5_joint,Axis_link5_joint,link5_joint,continuous,0,0,-0.254,3.1416,0,0,forearm,1,0,0,,,,,,,,,,,, +manipulator,5.8113236445223E-17,1.74020910368395E-17,-0.05715,0,0,0,0.237050487139156,0.000319176967769642,-3.08271195529153E-21,3.16873566981017E-20,0.000319176967769642,7.05373762522642E-20,0.000122196649085178,0,0,0,0,0,0,package://wroboarm_21/meshes/manipulator.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_21/meshes/manipulator.STL,,Manipulator-1,Origin_link6_joint,Axis_link6_joint,link6_joint,continuous,0,0,0.0254,3.1416,0,1.5708,wrist,0,0,1,,,,,,,,,,,, diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf new file mode 100644 index 00000000..e15f105b --- /dev/null +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -0,0 +1,365 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 05387d0cc98b9bc6f88e84a2e375108cfcbc9d87 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 10 Mar 2021 19:23:38 -0600 Subject: [PATCH 02/85] Added a wr_control_arm package. --- src/wr_control_arm/CMakeLists.txt | 205 ++++++++++++++++++++++++++++++ src/wr_control_arm/package.xml | 65 ++++++++++ 2 files changed, 270 insertions(+) create mode 100644 src/wr_control_arm/CMakeLists.txt create mode 100644 src/wr_control_arm/package.xml diff --git a/src/wr_control_arm/CMakeLists.txt b/src/wr_control_arm/CMakeLists.txt new file mode 100644 index 00000000..9846f7d8 --- /dev/null +++ b/src/wr_control_arm/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wr_control_arm) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES wr_control_arm +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/wr_control_arm.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/wr_control_arm_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_wr_control_arm.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/wr_control_arm/package.xml b/src/wr_control_arm/package.xml new file mode 100644 index 00000000..0e5a905e --- /dev/null +++ b/src/wr_control_arm/package.xml @@ -0,0 +1,65 @@ + + + wr_control_arm + 0.0.0 + Control systems subsystem package that drives the science subsystem. + + + + + E. Geng + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + roscpp + rospy + roscpp + rospy + + + + + + + + From 348b0c223a818d1d10332cdb7e8dd5f87c41ef81 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 10 Mar 2021 19:24:03 -0600 Subject: [PATCH 03/85] Created the Action server file. --- src/wr_control_arm/src/ArmControlSystem.cpp | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/wr_control_arm/src/ArmControlSystem.cpp diff --git a/src/wr_control_arm/src/ArmControlSystem.cpp b/src/wr_control_arm/src/ArmControlSystem.cpp new file mode 100644 index 00000000..fd0e6a6a --- /dev/null +++ b/src/wr_control_arm/src/ArmControlSystem.cpp @@ -0,0 +1 @@ +#include "ros.h" From 531e99aa93ea8f379ca12c9ed1e91b2c7c029d9c Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 10 Mar 2021 19:52:13 -0600 Subject: [PATCH 04/85] Added ArmControlSystem source file with default logic for a simple action server. --- src/wr_control_arm/src/ArmControlSystem.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/wr_control_arm/src/ArmControlSystem.cpp b/src/wr_control_arm/src/ArmControlSystem.cpp index fd0e6a6a..41e2017f 100644 --- a/src/wr_control_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_arm/src/ArmControlSystem.cpp @@ -1 +1,19 @@ -#include "ros.h" +#include "ros/ros.h" +#include +#include + +typedef actionlib::SimpleActionServer Server; + +void executeAction(const control_msgs::FollowJointTrajectoryActionGoalConstPtr& goal, Server* s){ + s->setSucceeded(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "ArmControlSystem"); + ros::NodeHandle n; + Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&executeAction, _1, &server), false); + server.start(); + ros::spin(); + return 0; +} \ No newline at end of file From f7f48246fa4eb1d41e6104a776e2db8fd0962cff Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 10 Mar 2021 19:52:57 -0600 Subject: [PATCH 05/85] Added wr_control_arm package build files, currently debugging a build error on catkin_make. --- src/wr_control_arm/CMakeLists.txt | 6 +++++- src/wr_control_arm/package.xml | 4 +++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/wr_control_arm/CMakeLists.txt b/src/wr_control_arm/CMakeLists.txt index 9846f7d8..c7ce6c30 100644 --- a/src/wr_control_arm/CMakeLists.txt +++ b/src/wr_control_arm/CMakeLists.txt @@ -10,6 +10,8 @@ project(wr_control_arm) find_package(catkin REQUIRED COMPONENTS roscpp rospy + actionlib + actionlib_msgs ) ## System dependencies are found with CMake's conventions @@ -104,7 +106,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( # INCLUDE_DIRS include # LIBRARIES wr_control_arm -# CATKIN_DEPENDS roscpp rospy + CATKIN_DEPENDS roscpp rospy message_runtime actionlib actionlib_msgs # DEPENDS system_lib ) @@ -133,6 +135,8 @@ include_directories( ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/wr_control_arm_node.cpp) +add_executable(ArmControlSystem src/ArmControlSystem.cpp) +target_link_libraries(ArmControlSystem ${catkin_LIBRARIES}) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_control_arm/package.xml b/src/wr_control_arm/package.xml index 0e5a905e..8f1aa6d7 100644 --- a/src/wr_control_arm/package.xml +++ b/src/wr_control_arm/package.xml @@ -43,7 +43,7 @@ - + message_runtime @@ -55,6 +55,8 @@ rospy roscpp rospy + actionlib + actionlib_msgs From eab1e58373caf3ed7286d5c728ea2914571c79d0 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 10 Mar 2021 19:57:36 -0600 Subject: [PATCH 06/85] Build succeeds on changes logged. --- src/wr_control_arm/src/ArmControlSystem.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/wr_control_arm/src/ArmControlSystem.cpp b/src/wr_control_arm/src/ArmControlSystem.cpp index 41e2017f..8cd07928 100644 --- a/src/wr_control_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_arm/src/ArmControlSystem.cpp @@ -4,15 +4,16 @@ typedef actionlib::SimpleActionServer Server; -void executeAction(const control_msgs::FollowJointTrajectoryActionGoalConstPtr& goal, Server* s){ - s->setSucceeded(); +void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { + // Do lots of awesome groundbreaking robot stuff here + as->setSucceeded(); } int main(int argc, char** argv) { ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; - Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&executeAction, _1, &server), false); + Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); server.start(); ros::spin(); return 0; From fdf5d67ec14253c7671b84fd275f15f4fb43522d Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 11 Mar 2021 19:30:31 -0600 Subject: [PATCH 07/85] Recompiled wroboarm_21 for ROS package detection. --- src/wroboarm_21/.setup_assistant | 2 +- src/wroboarm_21/config/sensors_3d.yaml | 9 +- src/wroboarm_21/export.log | 68 +-- src/wroboarm_21/launch/moveit.rviz | 614 ++++--------------------- src/wroboarm_21/package.xml | 2 +- 5 files changed, 131 insertions(+), 564 deletions(-) diff --git a/src/wroboarm_21/.setup_assistant b/src/wroboarm_21/.setup_assistant index 478660a7..04da6248 100644 --- a/src/wroboarm_21/.setup_assistant +++ b/src/wroboarm_21/.setup_assistant @@ -8,4 +8,4 @@ moveit_setup_assistant_config: CONFIG: author_name: me author_email: me@me.me - generated_timestamp: 1614898844 \ No newline at end of file + generated_timestamp: 1615512414 \ No newline at end of file diff --git a/src/wroboarm_21/config/sensors_3d.yaml b/src/wroboarm_21/config/sensors_3d.yaml index d2955dcd..a4bb13e3 100644 --- a/src/wroboarm_21/config/sensors_3d.yaml +++ b/src/wroboarm_21/config/sensors_3d.yaml @@ -1,3 +1,10 @@ # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it sensors: - - {} \ No newline at end of file + - filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater \ No newline at end of file diff --git a/src/wroboarm_21/export.log b/src/wroboarm_21/export.log index 9dafc806..052f4f98 100644 --- a/src/wroboarm_21/export.log +++ b/src/wroboarm_21/export.log @@ -11,12 +11,12 @@ 2020-12-21 21:22:47,349 INFO SwAddin.cs: 272 - Adding Part export to file menu 2020-12-21 21:22:47,350 INFO SwAddin.cs: 210 - Adding event handlers 2020-12-21 21:22:47,354 INFO SwAddin.cs: 217 - Connecting plugin to SolidWorks -2020-12-21 21:24:27,526 INFO SwAddin.cs: 294 - Assembly export called for file SimpleArm +2020-12-21 21:24:27,526 INFO SwAddin.cs: 294 - Assembly export called for file wroboarm_21 2020-12-21 21:24:29,650 INFO SwAddin.cs: 313 - Saving assembly 2020-12-21 21:24:29,826 INFO SwAddin.cs: 316 - Opening property manager -2020-12-21 21:24:29,876 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from SimpleArm -2020-12-21 21:24:29,891 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm -2020-12-21 21:24:29,893 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in SimpleArm +2020-12-21 21:24:29,876 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from wroboarm_21 +2020-12-21 21:24:29,891 INFO ExportHelperExtension.cs: 1136 - Found 59 in wroboarm_21 +2020-12-21 21:24:29,893 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in wroboarm_21 2020-12-21 21:24:29,894 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components 2020-12-21 21:24:29,896 INFO ExportHelperExtension.cs: 1160 - 7 components to check 2020-12-21 21:24:29,898 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forearm-1 @@ -40,9 +40,9 @@ 2020-12-21 21:24:29,924 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 2020-12-21 21:24:29,925 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 2020-12-21 21:24:29,928 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 -2020-12-21 21:24:29,930 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from SimpleArm -2020-12-21 21:24:29,932 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm -2020-12-21 21:24:29,934 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in SimpleArm +2020-12-21 21:24:29,930 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from wroboarm_21 +2020-12-21 21:24:29,932 INFO ExportHelperExtension.cs: 1136 - Found 59 in wroboarm_21 +2020-12-21 21:24:29,934 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in wroboarm_21 2020-12-21 21:24:29,935 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components 2020-12-21 21:24:29,936 INFO ExportHelperExtension.cs: 1160 - 7 components to check 2020-12-21 21:24:29,937 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forearm-1 @@ -76,31 +76,31 @@ 2020-12-21 21:24:30,485 INFO LinkNode.cs: 35 - Building node forearm 2020-12-21 21:24:30,486 INFO LinkNode.cs: 35 - Building node wrist 2020-12-21 21:24:30,486 INFO LinkNode.cs: 35 - Building node manipulator -2020-12-21 21:24:30,487 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for turntable from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,487 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for turntable from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,489 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???TurnTable-1@SimpleArm 2020-12-21 21:24:30,492 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\TurnTable.SLDPRT 2020-12-21 21:24:30,492 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link turntable -2020-12-21 21:24:30,493 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for gearbox from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,493 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for gearbox from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,493 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Shoulder-1@SimpleArm 2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Shoulder.SLDPRT 2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link gearbox -2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for upperarm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,494 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for upperarm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,495 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???UpperArm-1@SimpleArm 2020-12-21 21:24:30,495 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\UpperArm.SLDPRT 2020-12-21 21:24:30,495 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link upperarm -2020-12-21 21:24:30,496 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for elbow from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,496 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for elbow from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,496 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Elbow-1@SimpleArm( 2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Elbow.SLDPRT 2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link elbow -2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for forearm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,497 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for forearm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,498 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Forearm-1@SimpleArm* 2020-12-21 21:24:30,498 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Forearm.SLDPRT 2020-12-21 21:24:30,498 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link forearm -2020-12-21 21:24:30,499 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for wrist from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,499 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for wrist from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,499 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Wrist-1@SimpleArm1 2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Wrist.SLDPRT 2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link wrist -2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for manipulator from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:24:30,500 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for manipulator from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:24:30,501 INFO CommonSwOperations.cs: 245 - Loading component with PID ?.???Manipulator-1@SimpleArm5 2020-12-21 21:24:30,501 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Manipulator.SLDPRT 2020-12-21 21:24:30,502 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link manipulator @@ -109,13 +109,13 @@ 2020-12-21 21:24:52,099 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found nametrueturntablexyztrue-2.8254193696243868E-170-6.7465241345308752E-18rpytrue000originfalsefalsevaluetrue0.39680623003092352massfalseixxtrue0.00057145415023748434ixytrue0ixztrue2.2383483955223888E-21iyytrue0.0011002410492471767iyztrue0izztrue0.00057145415023748434inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametruegearboxxyztrue-6.8631413935065579E-180.03801532439663572-0.0013892205096921757rpytrue000originfalsefalsevaluetrue0.34120985304638518massfalseixxtrue0.00063726488338474081ixytrue7.0764513023917949E-20ixztrue-2.7252675378665934E-19iyytrue0.00083450439761804886iyztrue1.6294136810805574E-05izztrue0.00088931887892574886inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink1_jointtypetruecontinuousxyztrue000.0127rpytrue1.570800originfalsefalselinktrueturntableparenttruelinktruegearboxchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link1_joint2Origin_link1_joint2linktruenametrueupperarmxyztrue1.4123887173905075E-170.157206632847804122.7755575615628914E-17rpytrue000originfalsefalsevaluetrue1.264124215876306massfalseixxtrue0.013677990391055649ixytrue3.368445392332098E-19ixztrue3.9611983830710858E-20iyytrue0.000964387075304149iyztrue-2.6745670344635044E-18izztrue0.014091749673708917inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink2_jointtypetruecontinuousxyztrue00.072390.03683rpytrue000originfalsefalselinktruegearboxparenttruelinktrueupperarmchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link2_joint2Origin_link2_joint2linktruenametrueelbowxyztrue7.3321042375852733E-180.046279720003911486-0.0099170828579809855rpytrue000originfalsefalsevaluetrue0.625328404239412massfalseixxtrue0.0015093628788121875ixytrue7.2251958937028559E-20ixztrue2.6531119727066055E-20iyytrue0.00077150412582161528iyztrue-1.1346887107722466E-05izztrue0.0010232990551232335inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink3_jointtypetruecontinuousxyztrue00.3810rpytrue-1.570800originfalsefalselinktrueupperarmparenttruelinktrueelbowchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link3_jointOrigin_link3_jointlinktruenametrueforearmxyztrue-1.1893700871757326E-17-1.9081958235744878E-17-0.11306639219877124rpytrue000originfalsefalsevaluetrue0.46063240123192695massfalseixxtrue0.0022362056644483611ixytrue2.8623740668759855E-21ixztrue-6.9238813900453064E-20iyytrue0.0022468403158638628iyztrue-3.0218548775228389E-19izztrue0.00024560226807657237inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink4_jointtypetruecontinuousxyztrue00.044450.04445rpytrue3.141600originfalsefalselinktrueelbowparenttruelinktrueforearmchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link4_jointOrigin_link4_jointlinktruenametruewristxyztrue-7.1276563553462352E-097.8062556418956319E-18-3.4149891803281207E-09rpytrue000originfalsefalsevaluetrue0.064200890136229274massfalseixxtrue3.4980909309943769E-05ixytrue5.2887802729807491E-11ixztrue1.565944720011522E-18iyytrue2.4290927928157113E-05iyztrue-4.3708304367957221E-11izztrue2.8670971509987432E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink5_jointtypetruecontinuousxyztrue00-0.254rpytrue3.141600originfalsefalselinktrueforearmparenttruelinktruewristchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link5_jointOrigin_link5_jointlinktruenametruemanipulatorxyztrue3.4694469519536142E-189.8733856357333089E-18-0.038099999999999912rpytrue000originfalsefalsevaluetrue0.1580336580927709massfalseixxtrue0.00011719996250307502ixytrue2.2033040782480907E-21ixztrue2.5135101783302778E-20iyytrue0.00011719996250307503iyztrue8.5958526221282082E-21izztrue8.1464432723451653E-05inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruelink6_jointtypetruecontinuousxyztrue000.0254rpytrue3.141601.5708originfalsefalselinktruewristparenttruelinktruemanipulatorchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upperfalsesoft_lowerfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_link6_jointOrigin_link6_jointlinktruefalse4C4AAAUAAAD//v8XTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAA1AAAAfalsefalsefalse4C4AAAUAAAD//v8RVwByAGkAcwB0AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAxAAAAfalsefalsefalse4C4AAAUAAAD//v8TRgBvAHIAZQBhAHIAbQAtADEAQABTAGkAbQBwAGwAZQBBAHIAbQAEAAAAEAAAAAEAAAABAAAAKgAAAA==falsefalsefalse4C4AAAUAAAD//v8RRQBsAGIAbwB3AC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAoAAAAfalsefalsefalse4C4AAAUAAAD//v8UVQBwAHAAZQByAEEAcgBtAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAaAAAAfalsefalsefalse4C4AAAUAAAD//v8UUwBoAG8AdQBsAGQAZQByAC0AMQBAAFMAaQBtAHAAbABlAEEAcgBtAAQAAAAQAAAAAQAAAAEAAAAYAAAAfalsefalsefalse4C4AAAUAAAD//v8VVAB1AHIAbgBUAGEAYgBsAGUALQAxAEAAUwBpAG0AcABsAGUAQQByAG0ABAAAABAAAAABAAAAAQAAABkAAAA=falsefalse 2020-12-21 21:24:52,272 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message -2020-12-21 21:26:19,586 INFO SwAddin.cs: 294 - Assembly export called for file SimpleArm +2020-12-21 21:26:19,586 INFO SwAddin.cs: 294 - Assembly export called for file wroboarm_21 2020-12-21 21:26:19,586 INFO SwAddin.cs: 299 - Save is required 2020-12-21 21:26:19,587 INFO SwAddin.cs: 313 - Saving assembly 2020-12-21 21:26:20,090 INFO SwAddin.cs: 316 - Opening property manager -2020-12-21 21:26:20,091 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from SimpleArm -2020-12-21 21:26:20,092 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm -2020-12-21 21:26:20,093 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in SimpleArm +2020-12-21 21:26:20,091 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from wroboarm_21 +2020-12-21 21:26:20,092 INFO ExportHelperExtension.cs: 1136 - Found 59 in wroboarm_21 +2020-12-21 21:26:20,093 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in wroboarm_21 2020-12-21 21:26:20,094 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components 2020-12-21 21:26:20,095 INFO ExportHelperExtension.cs: 1160 - 7 components to check 2020-12-21 21:26:20,096 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forearm-1 @@ -139,9 +139,9 @@ 2020-12-21 21:26:20,123 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 2020-12-21 21:26:20,125 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 2020-12-21 21:26:20,127 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 -2020-12-21 21:26:20,128 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from SimpleArm -2020-12-21 21:26:20,129 INFO ExportHelperExtension.cs: 1136 - Found 59 in SimpleArm -2020-12-21 21:26:20,130 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in SimpleArm +2020-12-21 21:26:20,128 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from wroboarm_21 +2020-12-21 21:26:20,129 INFO ExportHelperExtension.cs: 1136 - Found 59 in wroboarm_21 +2020-12-21 21:26:20,130 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in wroboarm_21 2020-12-21 21:26:20,133 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components 2020-12-21 21:26:20,134 INFO ExportHelperExtension.cs: 1160 - 7 components to check 2020-12-21 21:26:20,135 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forearm-1 @@ -175,31 +175,31 @@ 2020-12-21 21:26:20,368 INFO LinkNode.cs: 35 - Building node forearm 2020-12-21 21:26:20,369 INFO LinkNode.cs: 35 - Building node wrist 2020-12-21 21:26:20,369 INFO LinkNode.cs: 35 - Building node manipulator -2020-12-21 21:26:20,370 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for turntable from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,370 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for turntable from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,371 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???TurnTable-1@SimpleArm 2020-12-21 21:26:20,372 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\TurnTable.SLDPRT 2020-12-21 21:26:20,373 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link turntable -2020-12-21 21:26:20,374 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for gearbox from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,374 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for gearbox from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,374 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Shoulder-1@SimpleArm 2020-12-21 21:26:20,376 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Shoulder.SLDPRT 2020-12-21 21:26:20,376 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link gearbox -2020-12-21 21:26:20,377 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for upperarm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,377 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for upperarm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,378 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???UpperArm-1@SimpleArm 2020-12-21 21:26:20,380 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\UpperArm.SLDPRT 2020-12-21 21:26:20,381 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link upperarm -2020-12-21 21:26:20,381 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for elbow from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,381 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for elbow from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,382 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Elbow-1@SimpleArm( 2020-12-21 21:26:20,383 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Elbow.SLDPRT 2020-12-21 21:26:20,384 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link elbow -2020-12-21 21:26:20,385 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for forearm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,385 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for forearm from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,386 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Forearm-1@SimpleArm* 2020-12-21 21:26:20,387 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Forearm.SLDPRT 2020-12-21 21:26:20,388 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link forearm -2020-12-21 21:26:20,390 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for wrist from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,390 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for wrist from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,391 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Wrist-1@SimpleArm1 2020-12-21 21:26:20,392 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Wrist.SLDPRT 2020-12-21 21:26:20,393 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link wrist -2020-12-21 21:26:20,395 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for manipulator from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\SimpleArm.SLDASM +2020-12-21 21:26:20,395 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for manipulator from C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\wroboarm_21.SLDASM 2020-12-21 21:26:20,395 INFO CommonSwOperations.cs: 245 - Loading component with PID ?2???Manipulator-1@SimpleArm5 2020-12-21 21:26:20,397 INFO CommonSwOperations.cs: 254 - Successfully loaded component C:\Users\ustuner\Desktop\GrabCAD\WRover20\am_system\am_resources\IK Model\Manipulator.SLDPRT 2020-12-21 21:26:20,398 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link manipulator @@ -235,9 +235,9 @@ 2020-12-21 21:26:42,283 WARN ExportHelperExtension.cs: 351 - Creating joint from parent forearm to child wrist failed 2020-12-21 21:26:42,290 INFO ExportHelperExtension.cs: 347 - Creating joint manipulator 2020-12-21 21:26:42,346 WARN ExportHelperExtension.cs: 351 - Creating joint from parent wrist to child manipulator failed -2020-12-21 21:26:42,543 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from SimpleArm -2020-12-21 21:26:42,552 INFO ExportHelperExtension.cs: 1136 - Found 63 in SimpleArm -2020-12-21 21:26:42,553 INFO ExportHelperExtension.cs: 1145 - Found 9 features of type [CoordSys] in SimpleArm +2020-12-21 21:26:42,543 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from wroboarm_21 +2020-12-21 21:26:42,552 INFO ExportHelperExtension.cs: 1136 - Found 63 in wroboarm_21 +2020-12-21 21:26:42,553 INFO ExportHelperExtension.cs: 1145 - Found 9 features of type [CoordSys] in wroboarm_21 2020-12-21 21:26:42,554 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components 2020-12-21 21:26:42,554 INFO ExportHelperExtension.cs: 1160 - 7 components to check 2020-12-21 21:26:42,554 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forearm-1 @@ -261,9 +261,9 @@ 2020-12-21 21:26:42,563 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 2020-12-21 21:26:42,564 INFO ExportHelperExtension.cs: 1136 - Found 29 in Wrist-1 2020-12-21 21:26:42,564 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 -2020-12-21 21:26:42,565 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from SimpleArm -2020-12-21 21:26:42,565 INFO ExportHelperExtension.cs: 1136 - Found 63 in SimpleArm -2020-12-21 21:26:42,566 INFO ExportHelperExtension.cs: 1145 - Found 8 features of type [RefAxis] in SimpleArm +2020-12-21 21:26:42,565 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from wroboarm_21 +2020-12-21 21:26:42,565 INFO ExportHelperExtension.cs: 1136 - Found 63 in wroboarm_21 +2020-12-21 21:26:42,566 INFO ExportHelperExtension.cs: 1145 - Found 8 features of type [RefAxis] in wroboarm_21 2020-12-21 21:26:42,566 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components 2020-12-21 21:26:42,569 INFO ExportHelperExtension.cs: 1160 - 7 components to check 2020-12-21 21:26:42,570 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forearm-1 diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index fe94253b..78189e60 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -5,8 +5,8 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - Splitter Ratio: 0.74256 - Tree Height: 664 + Splitter Ratio: 0.7425600290298462 + Tree Height: 241 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +14,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +27,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,302 +39,83 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Time: 5 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 0.5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + elbow: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_footprint: + forearm: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_link: + gearbox: Alpha: 1 Show Axes: false Show Trail: false Value: true - bl_caster_l_wheel_link: + manipulator: Alpha: 1 Show Axes: false Show Trail: false Value: true - bl_caster_r_wheel_link: + turntable: Alpha: 1 Show Axes: false Show Trail: false Value: true - bl_caster_rotation_link: + upperarm: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist: Alpha: 1 Show Axes: false Show Trail: false Value: true Loop Animation: true Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path Planning Metrics: Payload: 1 @@ -338,13 +123,14 @@ Visualization Manager: Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: left_arm + Planning Group: arm Query Goal State: true Query Start State: false Show Workspace: false @@ -355,300 +141,64 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.2 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: - base_bellow_link: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + elbow: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_footprint: + forearm: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_link: + gearbox: Alpha: 1 Show Axes: false Show Trail: false Value: true - bl_caster_l_wheel_link: + manipulator: Alpha: 1 Show Axes: false Show Trail: false Value: true - bl_caster_r_wheel_link: + turntable: Alpha: 1 Show Axes: false Show Trail: false Value: true - bl_caster_rotation_link: + upperarm: Alpha: 1 Show Axes: false Show Trail: false Value: true - br_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - double_stereo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_l_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_r_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_ir_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_kinect_rgb_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_mount_prosilica_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_plate_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - head_tilt_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser_tilt_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_elbow_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_forearm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_l_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_motor_accelerometer_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_palm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper_r_finger_tip_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_lift_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder_pan_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_arm_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_flex_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist_roll_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_mount_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_lift_link: + wrist: Alpha: 1 Show Axes: false Show Trail: false Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: turntable + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -659,31 +209,41 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9965 + Distance: 2.996500015258789 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 0.113567 - Y: 0.10592 - Z: 2.23518e-07 + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.509797 - Target Frame: /turntable - Value: XYOrbit (rviz) - Yaw: 5.65995 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5097969770431519 + Target Frame: turntable + Yaw: 5.659949779510498 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1337 + Height: 886 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - Motion Planning: + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000002a20000031cfc0200000007fb000000100044006900730070006c006100790073010000003d00000182000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c5000001940000018900ffffff000004900000031c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 459 - Y: -243 + Width: 1848 + X: 72 + Y: 27 diff --git a/src/wroboarm_21/package.xml b/src/wroboarm_21/package.xml index 4e45ea36..6d699a80 100644 --- a/src/wroboarm_21/package.xml +++ b/src/wroboarm_21/package.xml @@ -29,7 +29,7 @@ xacro - + From ba7afde55a688020d3cbd82b444b065a9e4c47fb Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 11 Mar 2021 19:31:21 -0600 Subject: [PATCH 08/85] Added a RViz launch file to test MoveIt/WRobot integration. --- src/wroboarm_21/launch/demo_test.launch | 64 +++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 src/wroboarm_21/launch/demo_test.launch diff --git a/src/wroboarm_21/launch/demo_test.launch b/src/wroboarm_21/launch/demo_test.launch new file mode 100644 index 00000000..a2ab9e84 --- /dev/null +++ b/src/wroboarm_21/launch/demo_test.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + From 41c39b7705bb2423f7d895f0df79214ba5f2c63f Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 11 Mar 2021 19:35:56 -0600 Subject: [PATCH 09/85] Moved wr_control_arm to pre-existing package wr_control_drive_arm and tested the SimpleActionServer with MoveIt configuration in the demo_test.launch wroboarm_21 launch file. MoveIt confirmed to be talking to the server and hearing the A-OK message. --- src/wr_control_arm/CMakeLists.txt | 209 ------------------ src/wr_control_arm/package.xml | 67 ------ .../src/ArmControlSystem.cpp | 1 + src/wr_control_drive_science/CMakeLists.txt | 8 + src/wr_control_drive_science/package.xml | 4 +- 5 files changed, 12 insertions(+), 277 deletions(-) delete mode 100644 src/wr_control_arm/CMakeLists.txt delete mode 100644 src/wr_control_arm/package.xml rename src/{wr_control_arm => wr_control_drive_arm}/src/ArmControlSystem.cpp (99%) diff --git a/src/wr_control_arm/CMakeLists.txt b/src/wr_control_arm/CMakeLists.txt deleted file mode 100644 index c7ce6c30..00000000 --- a/src/wr_control_arm/CMakeLists.txt +++ /dev/null @@ -1,209 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(wr_control_arm) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - actionlib - actionlib_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES wr_control_arm - CATKIN_DEPENDS roscpp rospy message_runtime actionlib actionlib_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/wr_control_arm.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/wr_control_arm_node.cpp) -add_executable(ArmControlSystem src/ArmControlSystem.cpp) -target_link_libraries(ArmControlSystem ${catkin_LIBRARIES}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_wr_control_arm.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/wr_control_arm/package.xml b/src/wr_control_arm/package.xml deleted file mode 100644 index 8f1aa6d7..00000000 --- a/src/wr_control_arm/package.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - wr_control_arm - 0.0.0 - Control systems subsystem package that drives the science subsystem. - - - - - E. Geng - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - message_runtime - - - - - catkin - roscpp - rospy - roscpp - rospy - roscpp - rospy - actionlib - actionlib_msgs - - - - - - - - diff --git a/src/wr_control_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp similarity index 99% rename from src/wr_control_arm/src/ArmControlSystem.cpp rename to src/wr_control_drive_arm/src/ArmControlSystem.cpp index 8cd07928..a8985d0a 100644 --- a/src/wr_control_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -9,6 +9,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server as->setSucceeded(); } + int main(int argc, char** argv) { ros::init(argc, argv, "ArmControlSystem"); diff --git a/src/wr_control_drive_science/CMakeLists.txt b/src/wr_control_drive_science/CMakeLists.txt index 2bc4d431..84ac42f7 100644 --- a/src/wr_control_drive_science/CMakeLists.txt +++ b/src/wr_control_drive_science/CMakeLists.txt @@ -10,6 +10,8 @@ project(wr_control_drive_science) find_package(catkin REQUIRED COMPONENTS roscpp rospy + actionlib + actionlib_msgs ) ## System dependencies are found with CMake's conventions @@ -132,7 +134,13 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide +<<<<<<< HEAD:src/wr_control_drive_science/CMakeLists.txt # add_executable(${PROJECT_NAME}_node src/wr_control_drive_science_node.cpp) +======= +# add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp) +add_executable(ArmControlSystem src/ArmControlSystem.cpp) +target_link_libraries(ArmControlSystem ${catkin_LIBRARIES}) +>>>>>>> Moved wr_control_arm to pre-existing package wr_control_drive_arm and tested the SimpleActionServer with MoveIt configuration in the demo_test.launch wroboarm_21 launch file. MoveIt confirmed to be talking to the server and hearing the A-OK message.:src/wr_control_drive_arm/CMakeLists.txt ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_control_drive_science/package.xml b/src/wr_control_drive_science/package.xml index de1573a2..8897923a 100644 --- a/src/wr_control_drive_science/package.xml +++ b/src/wr_control_drive_science/package.xml @@ -43,7 +43,7 @@ - + message_runtime @@ -55,6 +55,8 @@ rospy roscpp rospy + actionlib + actionlib_msgs From f47440da449fd0e5c6f0e05aa4902d2fc9876545 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 17 Mar 2021 20:02:53 -0500 Subject: [PATCH 10/85] Added an ArmMotor class to represent the per motor logic and abstract/avoid repitition on the details of driving to a position and setting power. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 66 +++++++++++++++++++++++ src/wr_control_drive_arm/src/ArmMotor.hpp | 18 +++++++ 2 files changed, 84 insertions(+) create mode 100644 src/wr_control_drive_arm/src/ArmMotor.cpp create mode 100644 src/wr_control_drive_arm/src/ArmMotor.hpp diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp new file mode 100644 index 00000000..ac3518e2 --- /dev/null +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -0,0 +1,66 @@ +#include "ArmMotor.hpp" +#include +#include "ros/ros.h" +#include "std_msgs/UInt32.h" +#include "std_msgs/UInt16.h" +#include "math.h" + +#define Std_UInt32 std_msgs::UInt32::ConstPtr& + +class ArmMotor{ + private: + enum MotorState{ + STOP, MOVING, RUN_TO_TARGET + }; + MotorState currState; + std::string motorName; + unsigned int controllerID; + unsigned int motorID; + ros::Subscriber encRead; + ros::Publisher speedPub; + int encoderVal; + + const int COUNTS_PER_ROTATION = 0; + + float radToEnc(float rads){ + return COUNTS_PER_ROTATION * rads / (2 * M_PI); + } + + void storeEncoderVals(const Std_UInt32 msg){ + this->encoderVal = msg->data; + } + + public: + ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle& n){ + if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is not valid on [0,3]"; + if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is not valid on [0,1]"; + + this->motorName = motorName; + this->controllerID = controllerID; + this->motorID = motorID; + this->currState = MotorState::STOP; + + std::string tpString = ((std::string)"/hsi/wroboclaw/aux") + std::to_string(controllerID); + + encRead = n.subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, storeEncoderVals, this); + speedPub = n.advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); + } + + // ~ArmMotor(); + + int getEncoderCounts(){ + return encoderVal; + } + + // void resetEncoder(); + void runToTarget(int targetCounts, float power){ + power = -1; + } + MotorState getMotorState(); + void setPower(float power){ + std_msgs::UInt16 msg; + msg.data = power * INT16_MAX; + speedPub.publish(msg); + currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; + }; +}; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp new file mode 100644 index 00000000..f5a72e14 --- /dev/null +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -0,0 +1,18 @@ +class ArmMotor{ + private: + enum MotorState{ + STOP, MOVING, RUN_TO_TARGET + }; + MotorState currState; + std::string motorName; + unsigned int controllerID; + unsigned int motorID; + public: + ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); + // ~ArmMotor(); + int getEncoderCounts(); + // void resetEncoder(); + void runToTarget(int targetCounts, float power); + MotorState getMotorState(); + void setPower(float power); +}; \ No newline at end of file From bbb683f764fd6fc5c0ca9ff0a4f3752f683b50d8 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 18 Mar 2021 20:08:29 -0500 Subject: [PATCH 11/85] More progress on ArmMotor.cpp, implemented simple runToTarget methods and server actions. --- .../src/ArmControlSystem.cpp | 11 +++++++ src/wr_control_drive_arm/src/ArmMotor.cpp | 33 ++++++++++++++++--- src/wr_control_drive_arm/src/ArmMotor.hpp | 1 + 3 files changed, 40 insertions(+), 5 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index a8985d0a..6f987baa 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -1,11 +1,22 @@ #include "ros/ros.h" #include #include +#include "ArmMotor.hpp" +ArmMotor motors[6]; typedef actionlib::SimpleActionServer Server; void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { // Do lots of awesome groundbreaking robot stuff here + //TODO: Execute arm actions in goal->trajectory.points[].position + for(int i = 0; i < goal->trajectory.points.size(); i++){ + trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; + for(int j = 0; j < currTargetPosition.positions.size(); j++){ + motors[j].runToTarget(currTargetPosition.positions[j], 0.1); + } + bool hasPositionFinished = false; + //TODO: Implement checking each motor for completed position actions. + } as->setSucceeded(); } diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index ac3518e2..26995281 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -53,14 +53,37 @@ class ArmMotor{ } // void resetEncoder(); + void runToTarget(int targetCounts, float power){ - power = -1; + this->runToTarget(targetCounts, power, false); + } + + bool hasReachedTarget(int targetCounts){ + return abs(targetCounts - this->getEncoderCounts()) < 10; } - MotorState getMotorState(); + + MotorState getMotorState(){ + return this->currState; + } + void setPower(float power){ + if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; + std_msgs::UInt16 msg; msg.data = power * INT16_MAX; - speedPub.publish(msg); - currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; - }; + this->speedPub.publish(msg); + this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; + } + + void runToTarget(int targetCounts, float power, bool block){ + if(targetCounts - this->getEncoderCounts() == 0) return; + power = abs(power) * abs(targetCounts - this->getEncoderCounts())/(targetCounts - this->getEncoderCounts()); + this->setPower(power); + this->currState = MotorState::RUN_TO_TARGET; + if(block){ + while(!this->hasReachedTarget(targetCounts)); + this->setPower(0.f); + this->currState = MotorState::STOP; + } + } }; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index f5a72e14..02126213 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -13,6 +13,7 @@ class ArmMotor{ int getEncoderCounts(); // void resetEncoder(); void runToTarget(int targetCounts, float power); + void runToTarget(int targetCounts, float power, bool block); MotorState getMotorState(); void setPower(float power); }; \ No newline at end of file From d1ae96faf1144818c0f9b02d099648e68c409615 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 22 Mar 2021 19:25:27 -0500 Subject: [PATCH 12/85] Control loop added, missing ROS logic (spins and such) and publishing TF/Joint State data. --- .../src/ArmControlSystem.cpp | 13 +++++++--- src/wr_control_drive_arm/src/ArmMotor.cpp | 24 ++++++++++++------- src/wr_control_drive_arm/src/ArmMotor.hpp | 7 +++--- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 6f987baa..7b08f168 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -7,15 +7,21 @@ ArmMotor motors[6]; typedef actionlib::SimpleActionServer Server; void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { - // Do lots of awesome groundbreaking robot stuff here - //TODO: Execute arm actions in goal->trajectory.points[].position for(int i = 0; i < goal->trajectory.points.size(); i++){ trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; for(int j = 0; j < currTargetPosition.positions.size(); j++){ motors[j].runToTarget(currTargetPosition.positions[j], 0.1); } + bool hasPositionFinished = false; - //TODO: Implement checking each motor for completed position actions. + while(!hasPositionFinished){ + bool temp = true; + for(int j = 0; j < currTargetPosition.positions.size(); j++){ + motors[j].runToTarget(currTargetPosition.positions[j], 0.1); + temp &= motors[j].getMotorState() == MotorState::STOP; + } + hasPositionFinished = temp; + } } as->setSucceeded(); } @@ -23,6 +29,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server int main(int argc, char** argv) { + ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 26995281..1f3ce3d7 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -9,9 +9,6 @@ class ArmMotor{ private: - enum MotorState{ - STOP, MOVING, RUN_TO_TARGET - }; MotorState currState; std::string motorName; unsigned int controllerID; @@ -20,9 +17,9 @@ class ArmMotor{ ros::Publisher speedPub; int encoderVal; - const int COUNTS_PER_ROTATION = 0; + static const int COUNTS_PER_ROTATION = UINT_FAST32_MAX; - float radToEnc(float rads){ + static float radToEnc(float rads){ return COUNTS_PER_ROTATION * rads / (2 * M_PI); } @@ -76,14 +73,23 @@ class ArmMotor{ } void runToTarget(int targetCounts, float power, bool block){ - if(targetCounts - this->getEncoderCounts() == 0) return; - power = abs(power) * abs(targetCounts - this->getEncoderCounts())/(targetCounts - this->getEncoderCounts()); - this->setPower(power); - this->currState = MotorState::RUN_TO_TARGET; + if(this->getMotorState() != MotorState::RUN_TO_TARGET){ + power = abs(power) * abs(targetCounts - this->getEncoderCounts())/(targetCounts - this->getEncoderCounts()); + this->setPower(power); + this->currState = MotorState::RUN_TO_TARGET; + } + if(this->hasReachedTarget(targetCounts)){ + this->setPower(0.f); + this->currState = MotorState::STOP; + } if(block){ while(!this->hasReachedTarget(targetCounts)); this->setPower(0.f); this->currState = MotorState::STOP; } } + + void runToTarget(double rads, float power){ + runToTarget(this->radToEnc(rads), power, false); + } }; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 02126213..fec5db22 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -1,8 +1,8 @@ -class ArmMotor{ - private: - enum MotorState{ +enum MotorState{ STOP, MOVING, RUN_TO_TARGET }; +class ArmMotor{ + private: MotorState currState; std::string motorName; unsigned int controllerID; @@ -14,6 +14,7 @@ class ArmMotor{ // void resetEncoder(); void runToTarget(int targetCounts, float power); void runToTarget(int targetCounts, float power, bool block); + void runToTarget(double rads, float power); MotorState getMotorState(); void setPower(float power); }; \ No newline at end of file From 49a955ff18097d958d0d2fcd7d35d4f4de43fd31 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 22 Mar 2021 19:57:57 -0500 Subject: [PATCH 13/85] Added framework of publishing Joint State data. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 10 ++++++++++ src/wr_control_drive_arm/src/ArmMotor.cpp | 4 ++++ src/wr_control_drive_arm/src/ArmMotor.hpp | 1 + 3 files changed, 15 insertions(+) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 7b08f168..0335d5e9 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -1,9 +1,11 @@ #include "ros/ros.h" #include #include +#include #include "ArmMotor.hpp" ArmMotor motors[6]; +ros::Publisher jointStatePublisher; typedef actionlib::SimpleActionServer Server; void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { @@ -14,12 +16,19 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server } bool hasPositionFinished = false; + std::vector names(); + std::vector positions(); while(!hasPositionFinished){ bool temp = true; + sensor_msgs::JointState* js_msg; + names.clear(); + positions.clear(); for(int j = 0; j < currTargetPosition.positions.size(); j++){ motors[j].runToTarget(currTargetPosition.positions[j], 0.1); temp &= motors[j].getMotorState() == MotorState::STOP; } + //TODO: Set js_msg + jointStatePublisher.publish(js_msg); hasPositionFinished = temp; } } @@ -32,6 +41,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; + jointStatePublisher = n.advertise("", 1000); Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); server.start(); ros::spin(); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 1f3ce3d7..4ede6844 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -92,4 +92,8 @@ class ArmMotor{ void runToTarget(double rads, float power){ runToTarget(this->radToEnc(rads), power, false); } + + double getRads(){ + return this->getEncoderCounts() * 2 * M_PI / COUNTS_PER_ROTATION; + } }; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index fec5db22..9f02418e 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -17,4 +17,5 @@ class ArmMotor{ void runToTarget(double rads, float power); MotorState getMotorState(); void setPower(float power); + double getRads(); }; \ No newline at end of file From a962e84c35d1d17a0fc357dd47c0df1bca103d90 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 22 Mar 2021 20:07:56 -0500 Subject: [PATCH 14/85] Moved and resorted the CMakeLists and package files for the arm and science control subsystems. --- src/wr_control_drive_arm/CMakeLists.txt | 209 ++++++++++++++++++++ src/wr_control_drive_arm/package.xml | 67 +++++++ src/wr_control_drive_science/CMakeLists.txt | 8 - src/wr_control_drive_science/package.xml | 2 - 4 files changed, 276 insertions(+), 10 deletions(-) create mode 100644 src/wr_control_drive_arm/CMakeLists.txt create mode 100644 src/wr_control_drive_arm/package.xml diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt new file mode 100644 index 00000000..85fadf6a --- /dev/null +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -0,0 +1,209 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wr_control_drive_arm) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + actionlib + actionlib_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES wr_control_drive_science +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/wr_control_drive_science.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp) +add_executable(ArmControlSystem src/ArmControlSystem.cpp) +target_link_libraries(ArmControlSystem ${catkin_LIBRARIES}) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_wr_control_drive_science.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/wr_control_drive_arm/package.xml b/src/wr_control_drive_arm/package.xml new file mode 100644 index 00000000..87fc3043 --- /dev/null +++ b/src/wr_control_drive_arm/package.xml @@ -0,0 +1,67 @@ + + + wr_control_drive_arm + 0.0.0 + Control systems subsystem package that drives the arm subsystem. + + + + + E. Geng + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + message_runtime + + + + + catkin + roscpp + rospy + roscpp + rospy + roscpp + rospy + actionlib + actionlib_msgs + + + + + + + + diff --git a/src/wr_control_drive_science/CMakeLists.txt b/src/wr_control_drive_science/CMakeLists.txt index 84ac42f7..68304cbe 100644 --- a/src/wr_control_drive_science/CMakeLists.txt +++ b/src/wr_control_drive_science/CMakeLists.txt @@ -10,8 +10,6 @@ project(wr_control_drive_science) find_package(catkin REQUIRED COMPONENTS roscpp rospy - actionlib - actionlib_msgs ) ## System dependencies are found with CMake's conventions @@ -134,13 +132,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -<<<<<<< HEAD:src/wr_control_drive_science/CMakeLists.txt -# add_executable(${PROJECT_NAME}_node src/wr_control_drive_science_node.cpp) -======= # add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp) -add_executable(ArmControlSystem src/ArmControlSystem.cpp) -target_link_libraries(ArmControlSystem ${catkin_LIBRARIES}) ->>>>>>> Moved wr_control_arm to pre-existing package wr_control_drive_arm and tested the SimpleActionServer with MoveIt configuration in the demo_test.launch wroboarm_21 launch file. MoveIt confirmed to be talking to the server and hearing the A-OK message.:src/wr_control_drive_arm/CMakeLists.txt ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_control_drive_science/package.xml b/src/wr_control_drive_science/package.xml index 8897923a..f2d77f2c 100644 --- a/src/wr_control_drive_science/package.xml +++ b/src/wr_control_drive_science/package.xml @@ -55,8 +55,6 @@ rospy roscpp rospy - actionlib - actionlib_msgs From 95cd5f56ac8b2ab6f4a03a902a7255a27f6bb96e Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 25 Mar 2021 19:14:50 -0500 Subject: [PATCH 15/85] Modified the demo_test.launch file to run the 'real' robot and read joint states from the right source. --- src/wroboarm_21/launch/demo_test.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/wroboarm_21/launch/demo_test.launch b/src/wroboarm_21/launch/demo_test.launch index a2ab9e84..5d722af0 100644 --- a/src/wroboarm_21/launch/demo_test.launch +++ b/src/wroboarm_21/launch/demo_test.launch @@ -32,10 +32,10 @@ - [move_group/fake_controller_joint_states] + [control/arm_joint_states] - [move_group/fake_controller_joint_states] + [control/arm_joint_states] From ce6b18aa33a08d02daf7b4a94a397ff85a78aa2f Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 25 Mar 2021 19:16:13 -0500 Subject: [PATCH 16/85] Updated source code to correctly implement header functions, avoid duplicate definition, include all methods used, and compile. --- .../src/ArmControlSystem.cpp | 25 ++- src/wr_control_drive_arm/src/ArmMotor.cpp | 180 +++++++++--------- src/wr_control_drive_arm/src/ArmMotor.hpp | 19 +- 3 files changed, 122 insertions(+), 102 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 0335d5e9..02428838 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -16,18 +16,25 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server } bool hasPositionFinished = false; - std::vector names(); - std::vector positions(); + std::vector names; + std::vector positions; while(!hasPositionFinished){ bool temp = true; - sensor_msgs::JointState* js_msg; + sensor_msgs::JointState js_msg; + names.clear(); positions.clear(); + for(int j = 0; j < currTargetPosition.positions.size(); j++){ motors[j].runToTarget(currTargetPosition.positions[j], 0.1); temp &= motors[j].getMotorState() == MotorState::STOP; + names.push_back(motors[j].getMotorName()); + positions.push_back(motors[j].getRads()); } //TODO: Set js_msg + js_msg.name = names; + js_msg.position = positions; + jointStatePublisher.publish(js_msg); hasPositionFinished = temp; } @@ -38,11 +45,19 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server int main(int argc, char** argv) { - ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; - jointStatePublisher = n.advertise("", 1000); + + motors[0] = ArmMotor("link1_joint", 0, 0, &n); + motors[1] = ArmMotor("link2_joint", 0, 1, &n); + motors[2] = ArmMotor("link3_joint", 1, 0, &n); + motors[3] = ArmMotor("link4_joint", 1, 1, &n); + motors[4] = ArmMotor("link5_joint", 2, 0, &n); + motors[5] = ArmMotor("link6_joint", 2, 1, &n); + + jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); + server.start(); ros::spin(); return 0; diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 4ede6844..96738753 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -1,99 +1,89 @@ #include "ArmMotor.hpp" -#include -#include "ros/ros.h" -#include "std_msgs/UInt32.h" -#include "std_msgs/UInt16.h" -#include "math.h" #define Std_UInt32 std_msgs::UInt32::ConstPtr& -class ArmMotor{ - private: - MotorState currState; - std::string motorName; - unsigned int controllerID; - unsigned int motorID; - ros::Subscriber encRead; - ros::Publisher speedPub; - int encoderVal; - - static const int COUNTS_PER_ROTATION = UINT_FAST32_MAX; - - static float radToEnc(float rads){ - return COUNTS_PER_ROTATION * rads / (2 * M_PI); - } - - void storeEncoderVals(const Std_UInt32 msg){ - this->encoderVal = msg->data; - } - - public: - ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle& n){ - if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is not valid on [0,3]"; - if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is not valid on [0,1]"; - - this->motorName = motorName; - this->controllerID = controllerID; - this->motorID = motorID; - this->currState = MotorState::STOP; - - std::string tpString = ((std::string)"/hsi/wroboclaw/aux") + std::to_string(controllerID); - - encRead = n.subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, storeEncoderVals, this); - speedPub = n.advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); - } - - // ~ArmMotor(); - - int getEncoderCounts(){ - return encoderVal; - } - - // void resetEncoder(); - - void runToTarget(int targetCounts, float power){ - this->runToTarget(targetCounts, power, false); - } - - bool hasReachedTarget(int targetCounts){ - return abs(targetCounts - this->getEncoderCounts()) < 10; - } - - MotorState getMotorState(){ - return this->currState; - } - - void setPower(float power){ - if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; - - std_msgs::UInt16 msg; - msg.data = power * INT16_MAX; - this->speedPub.publish(msg); - this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; - } - - void runToTarget(int targetCounts, float power, bool block){ - if(this->getMotorState() != MotorState::RUN_TO_TARGET){ - power = abs(power) * abs(targetCounts - this->getEncoderCounts())/(targetCounts - this->getEncoderCounts()); - this->setPower(power); - this->currState = MotorState::RUN_TO_TARGET; - } - if(this->hasReachedTarget(targetCounts)){ - this->setPower(0.f); - this->currState = MotorState::STOP; - } - if(block){ - while(!this->hasReachedTarget(targetCounts)); - this->setPower(0.f); - this->currState = MotorState::STOP; - } - } - - void runToTarget(double rads, float power){ - runToTarget(this->radToEnc(rads), power, false); - } - - double getRads(){ - return this->getEncoderCounts() * 2 * M_PI / COUNTS_PER_ROTATION; - } -}; \ No newline at end of file +static const int COUNTS_PER_ROTATION = UINT_FAST32_MAX; + +int ArmMotor::radToEnc(float rads){ + return COUNTS_PER_ROTATION * rads / (2 * M_PI); +} + +void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ + this->encoderVal = msg->data; +} + + +ArmMotor::ArmMotor(){} + +ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n){ + if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is not valid on [0,3]"; + if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is not valid on [0,1]"; + + this->motorName = motorName; + this->controllerID = controllerID; + this->motorID = motorID; + this->currState = MotorState::STOP; + + std::string tpString = ((std::string)"/hsi/wroboclaw/aux") + std::to_string(controllerID); + + this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); + this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); +} + +// ~ArmMotor(); + +int ArmMotor::getEncoderCounts(){ + return this->encoderVal; +} + +// void resetEncoder(); + +void ArmMotor::runToTarget(int targetCounts, float power){ + this->runToTarget(targetCounts, power, false); +} + +bool ArmMotor::hasReachedTarget(int targetCounts){ + return abs(targetCounts - this->getEncoderCounts()) < 10; +} + +MotorState ArmMotor::getMotorState(){ + return this->currState; +} + +void ArmMotor::setPower(float power){ + if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; + + std_msgs::UInt16 msg; + msg.data = power * INT16_MAX; + this->speedPub.publish(msg); + this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; +} + +void ArmMotor::runToTarget(int targetCounts, float power, bool block){ + if(this->getMotorState() != MotorState::RUN_TO_TARGET){ + power = abs(power) * abs(targetCounts - this->getEncoderCounts())/(targetCounts - this->getEncoderCounts()); + this->setPower(power); + this->currState = MotorState::RUN_TO_TARGET; + } + if(this->hasReachedTarget(targetCounts)){ + this->setPower(0.f); + this->currState = MotorState::STOP; + } + if(block){ + while(!this->hasReachedTarget(targetCounts)); + this->setPower(0.f); + this->currState = MotorState::STOP; + } +} + +void ArmMotor::runToTarget(double rads, float power){ + runToTarget(this->radToEnc(rads), power, false); +} + +double ArmMotor::getRads(){ + return this->getEncoderCounts() * 2 * M_PI / COUNTS_PER_ROTATION; +} + +std::string ArmMotor::getMotorName(){ + return this->motorName; +} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 9f02418e..a3952769 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -1,13 +1,26 @@ +#include +#include "ros/ros.h" +#include "std_msgs/UInt32.h" +#include "std_msgs/UInt16.h" +#include "math.h" +#include + enum MotorState{ - STOP, MOVING, RUN_TO_TARGET - }; + STOP, MOVING, RUN_TO_TARGET +}; class ArmMotor{ private: MotorState currState; std::string motorName; unsigned int controllerID; unsigned int motorID; + unsigned int encoderVal; + ros::Subscriber encRead; + ros::Publisher speedPub; + static int radToEnc(float rad); + void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); public: + ArmMotor(); ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); // ~ArmMotor(); int getEncoderCounts(); @@ -18,4 +31,6 @@ class ArmMotor{ MotorState getMotorState(); void setPower(float power); double getRads(); + std::string getMotorName(); + bool hasReachedTarget(int targetCounts); }; \ No newline at end of file From c6c629a009bec0f708d0afc2454b7b1994bf7848 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 25 Mar 2021 19:17:15 -0500 Subject: [PATCH 17/85] Modified CMakeLists and package.xml to depend on the new sensor_msgs needed for publishing Joint State data and to compile ArmControlSystem.cpp and ArmMotor.cpp together. --- src/wr_control_drive_arm/CMakeLists.txt | 10 ++++++---- src/wr_control_drive_arm/package.xml | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 85fadf6a..cc3b0029 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS rospy actionlib actionlib_msgs + sensor_msgs ) ## System dependencies are found with CMake's conventions @@ -122,9 +123,10 @@ include_directories( ) ## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/wr_control_drive_science.cpp -# ) + add_library( ${PROJECT_NAME} + src/ArmControlSystem.cpp + src/ArmMotor.cpp + ) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -136,7 +138,7 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp) add_executable(ArmControlSystem src/ArmControlSystem.cpp) -target_link_libraries(ArmControlSystem ${catkin_LIBRARIES}) +target_link_libraries(ArmControlSystem ${catkin_LIBRARIES} ${PROJECT_NAME}) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_control_drive_arm/package.xml b/src/wr_control_drive_arm/package.xml index 87fc3043..9b0ad0c8 100644 --- a/src/wr_control_drive_arm/package.xml +++ b/src/wr_control_drive_arm/package.xml @@ -57,7 +57,7 @@ rospy actionlib actionlib_msgs - + sensor_msgs From 94054954a44be298ec124f37dd3c2cb754037be0 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 25 Mar 2021 20:28:49 -0500 Subject: [PATCH 18/85] Re-referenced the wroboclaw topics; wroboclaw topics aren't spelled with the leading 'w'. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 96738753..2aa82793 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -24,7 +24,7 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in this->motorID = motorID; this->currState = MotorState::STOP; - std::string tpString = ((std::string)"/hsi/wroboclaw/aux") + std::to_string(controllerID); + std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); From fb48dd2f838f3dab8a431717a329363ae240e4e6 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 29 Mar 2021 18:46:40 -0500 Subject: [PATCH 19/85] Updated motor power message types for the arm control system to use Int16 and not UInt16. Patched the warning of integer overflow for the COUNTS_PER_ROTATION constant. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 6 +++--- src/wr_control_drive_arm/src/ArmMotor.hpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 2aa82793..3e3f5058 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -2,7 +2,7 @@ #define Std_UInt32 std_msgs::UInt32::ConstPtr& -static const int COUNTS_PER_ROTATION = UINT_FAST32_MAX; +static const unsigned long int COUNTS_PER_ROTATION = UINT_FAST32_MAX; int ArmMotor::radToEnc(float rads){ return COUNTS_PER_ROTATION * rads / (2 * M_PI); @@ -27,7 +27,7 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); - this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); + this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); } // ~ArmMotor(); @@ -53,7 +53,7 @@ MotorState ArmMotor::getMotorState(){ void ArmMotor::setPower(float power){ if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; - std_msgs::UInt16 msg; + std_msgs::Int16 msg; msg.data = power * INT16_MAX; this->speedPub.publish(msg); this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index a3952769..06256e58 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -1,7 +1,7 @@ #include #include "ros/ros.h" #include "std_msgs/UInt32.h" -#include "std_msgs/UInt16.h" +#include "std_msgs/Int16.h" #include "math.h" #include From af687861e9d0942cf394ce882f9123c6b595a0d0 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 31 Mar 2021 20:12:19 -0500 Subject: [PATCH 20/85] Updated ArmMotor to fix ArmControlSystem encoder issue; patch reached by making the ArmMotor array into an ArmMotor* array, which avoids instantiation entirely until necessary. --- src/wr_control_drive_arm/CMakeLists.txt | 4 +- .../src/ArmControlSystem.cpp | 32 +++++++----- src/wr_control_drive_arm/src/ArmMotor.cpp | 19 ++++--- src/wr_control_drive_arm/src/ArmMotor.hpp | 6 ++- src/wr_control_drive_arm/src/SimpleACS.cpp | 50 +++++++++++++++++++ src/wr_entry_point/launch/test_arm.launch | 4 ++ src/wroboarm_21/launch/moveit.rviz | 4 +- 7 files changed, 96 insertions(+), 23 deletions(-) create mode 100644 src/wr_control_drive_arm/src/SimpleACS.cpp create mode 100644 src/wr_entry_point/launch/test_arm.launch diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index cc3b0029..b2074ede 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -124,7 +124,6 @@ include_directories( ## Declare a C++ library add_library( ${PROJECT_NAME} - src/ArmControlSystem.cpp src/ArmMotor.cpp ) @@ -140,6 +139,9 @@ include_directories( add_executable(ArmControlSystem src/ArmControlSystem.cpp) target_link_libraries(ArmControlSystem ${catkin_LIBRARIES} ${PROJECT_NAME}) +add_executable(SimpleACS src/SimpleACS.cpp) +target_link_libraries(SimpleACS ${catkin_LIBRARIES} ${PROJECT_NAME}) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 02428838..9ee1f532 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -4,7 +4,7 @@ #include #include "ArmMotor.hpp" -ArmMotor motors[6]; +ArmMotor *motors[6]; ros::Publisher jointStatePublisher; typedef actionlib::SimpleActionServer Server; @@ -12,12 +12,13 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server for(int i = 0; i < goal->trajectory.points.size(); i++){ trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; for(int j = 0; j < currTargetPosition.positions.size(); j++){ - motors[j].runToTarget(currTargetPosition.positions[j], 0.1); + motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); } bool hasPositionFinished = false; std::vector names; std::vector positions; + unsigned long long int i2 = 0; while(!hasPositionFinished){ bool temp = true; sensor_msgs::JointState js_msg; @@ -25,11 +26,18 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server names.clear(); positions.clear(); + if(i2%10000==0){ + std::cout<<"Motor0 Target (rads):\t"<getEncoderCounts()<getRads()<runToTarget(currTargetPosition.positions[j], 0.1); + temp &= motors[j]->getMotorState() == MotorState::STOP; + names.push_back(motors[j]->getMotorName()); + positions.push_back(motors[j]->getRads()); } //TODO: Set js_msg js_msg.name = names; @@ -48,12 +56,12 @@ int main(int argc, char** argv) ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; - motors[0] = ArmMotor("link1_joint", 0, 0, &n); - motors[1] = ArmMotor("link2_joint", 0, 1, &n); - motors[2] = ArmMotor("link3_joint", 1, 0, &n); - motors[3] = ArmMotor("link4_joint", 1, 1, &n); - motors[4] = ArmMotor("link5_joint", 2, 0, &n); - motors[5] = ArmMotor("link6_joint", 2, 1, &n); + motors[0] = new ArmMotor("link1_joint", 0, 0, n); + motors[1] = new ArmMotor("link2_joint", 0, 1, n); + motors[2] = new ArmMotor("link3_joint", 1, 0, n); + motors[3] = new ArmMotor("link4_joint", 1, 1, n); + motors[4] = new ArmMotor("link5_joint", 2, 0, n); + motors[5] = new ArmMotor("link6_joint", 2, 1, n); jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 3e3f5058..a1690385 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -9,13 +9,16 @@ int ArmMotor::radToEnc(float rads){ } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ + // std::cout<<"I ran "/*mtr:"<getMotorName()<<*/" val: "<data<encoderVal = msg->data; + // std::cout<<"value stored: "<encoderVal<getEncoderCounts()< 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is not valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is not valid on [0,1]"; @@ -26,8 +29,8 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); - this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); - this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); + this->encRead = n.subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); + this->speedPub = n.advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); } // ~ArmMotor(); @@ -53,9 +56,9 @@ MotorState ArmMotor::getMotorState(){ void ArmMotor::setPower(float power){ if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; - std_msgs::Int16 msg; - msg.data = power * INT16_MAX; - this->speedPub.publish(msg); + this->powerMsg = new std_msgs::Int16(); + this->powerMsg->data = power * INT16_MAX; + this->speedPub.publish(*(this->powerMsg)); this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; } @@ -86,4 +89,8 @@ double ArmMotor::getRads(){ std::string ArmMotor::getMotorName(){ return this->motorName; +} + +float ArmMotor::getPower(){ + return ((float)this->powerMsg->data)/INT16_MAX; } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 06256e58..613bacab 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -14,14 +14,15 @@ class ArmMotor{ std::string motorName; unsigned int controllerID; unsigned int motorID; - unsigned int encoderVal; + unsigned long int encoderVal; ros::Subscriber encRead; ros::Publisher speedPub; + std_msgs::Int16 *powerMsg; static int radToEnc(float rad); void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); public: ArmMotor(); - ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); + ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle n); // ~ArmMotor(); int getEncoderCounts(); // void resetEncoder(); @@ -33,4 +34,5 @@ class ArmMotor{ double getRads(); std::string getMotorName(); bool hasReachedTarget(int targetCounts); + float getPower(); }; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleACS.cpp b/src/wr_control_drive_arm/src/SimpleACS.cpp new file mode 100644 index 00000000..9028a7eb --- /dev/null +++ b/src/wr_control_drive_arm/src/SimpleACS.cpp @@ -0,0 +1,50 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include "ArmMotor.hpp" + +ArmMotor* motors[6]; +ros::Publisher jointStatePublisher; +typedef actionlib::SimpleActionServer Server; + +void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { + // for(int i = 0; i < goal->trajectory.points.size(); i++){ + // trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; + + + // } + motors[0]->setPower(0.01); + int i = 0; + ros::Rate sleeper(50); + while(i++ < 100){ + std::cout<getEncoderCounts()<getPower()<setSucceeded(); +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "ArmControlSystem"); + ros::NodeHandle n; + + motors[0] = new ArmMotor("link1_joint", 0, 0, n); + motors[1] = new ArmMotor("link2_joint", 0, 1, n); + motors[2] = new ArmMotor("link3_joint", 1, 0, n); + motors[3] = new ArmMotor("link4_joint", 1, 1, n); + motors[4] = new ArmMotor("link5_joint", 2, 0, n); + motors[5] = new ArmMotor("link6_joint", 2, 1, n); + + jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); + Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); + + server.start(); + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/src/wr_entry_point/launch/test_arm.launch b/src/wr_entry_point/launch/test_arm.launch new file mode 100644 index 00000000..7ae0bf5c --- /dev/null +++ b/src/wr_entry_point/launch/test_arm.launch @@ -0,0 +1,4 @@ + + + + diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index 78189e60..e859664b 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -225,9 +225,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5097969770431519 + Pitch: 0.4297969341278076 Target Frame: turntable - Yaw: 5.659949779510498 + Yaw: 5.694949150085449 Saved: ~ Window Geometry: Displays: From 248dd2124739d54682df5c32f900c1dd199d87c2 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 16:30:15 -0500 Subject: [PATCH 21/85] Modified the ArmMotor constructor to accept NodeHandle pointers rather than NodeHandles themselves. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 12 ++++++------ src/wr_control_drive_arm/src/ArmMotor.cpp | 6 +++--- src/wr_control_drive_arm/src/ArmMotor.hpp | 2 +- src/wr_control_drive_arm/src/SimpleACS.cpp | 12 ++++++------ 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 9ee1f532..b02cc699 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -56,12 +56,12 @@ int main(int argc, char** argv) ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; - motors[0] = new ArmMotor("link1_joint", 0, 0, n); - motors[1] = new ArmMotor("link2_joint", 0, 1, n); - motors[2] = new ArmMotor("link3_joint", 1, 0, n); - motors[3] = new ArmMotor("link4_joint", 1, 1, n); - motors[4] = new ArmMotor("link5_joint", 2, 0, n); - motors[5] = new ArmMotor("link6_joint", 2, 1, n); + motors[0] = new ArmMotor("link1_joint", 0, 0, &n); + motors[1] = new ArmMotor("link2_joint", 0, 1, &n); + motors[2] = new ArmMotor("link3_joint", 1, 0, &n); + motors[3] = new ArmMotor("link4_joint", 1, 1, &n); + motors[4] = new ArmMotor("link5_joint", 2, 0, &n); + motors[5] = new ArmMotor("link6_joint", 2, 1, &n); jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index a1690385..1f5f91f8 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -18,7 +18,7 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ ArmMotor::ArmMotor(){} -ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle n){ +ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n){ if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is not valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is not valid on [0,1]"; @@ -29,8 +29,8 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); - this->encRead = n.subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); - this->speedPub = n.advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); + this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); + this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); } // ~ArmMotor(); diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 613bacab..58acccf1 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -22,7 +22,7 @@ class ArmMotor{ void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); public: ArmMotor(); - ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle n); + ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); // ~ArmMotor(); int getEncoderCounts(); // void resetEncoder(); diff --git a/src/wr_control_drive_arm/src/SimpleACS.cpp b/src/wr_control_drive_arm/src/SimpleACS.cpp index 9028a7eb..1c805c28 100644 --- a/src/wr_control_drive_arm/src/SimpleACS.cpp +++ b/src/wr_control_drive_arm/src/SimpleACS.cpp @@ -34,12 +34,12 @@ int main(int argc, char** argv) ros::init(argc, argv, "ArmControlSystem"); ros::NodeHandle n; - motors[0] = new ArmMotor("link1_joint", 0, 0, n); - motors[1] = new ArmMotor("link2_joint", 0, 1, n); - motors[2] = new ArmMotor("link3_joint", 1, 0, n); - motors[3] = new ArmMotor("link4_joint", 1, 1, n); - motors[4] = new ArmMotor("link5_joint", 2, 0, n); - motors[5] = new ArmMotor("link6_joint", 2, 1, n); + motors[0] = new ArmMotor("link1_joint", 0, 0, &n); + motors[1] = new ArmMotor("link2_joint", 0, 1, &n); + motors[2] = new ArmMotor("link3_joint", 1, 0, &n); + motors[3] = new ArmMotor("link4_joint", 1, 1, &n); + motors[4] = new ArmMotor("link5_joint", 2, 0, &n); + motors[5] = new ArmMotor("link6_joint", 2, 1, &n); jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); From 99c4fb1655cc12ce492e3356bd8209c053cba228 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 16:31:26 -0500 Subject: [PATCH 22/85] Changed ArmMotor constructor parameter error messages to more accurately reflect the problem described. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 1f5f91f8..b0f449c8 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -19,8 +19,8 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ ArmMotor::ArmMotor(){} ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n){ - if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is not valid on [0,3]"; - if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is not valid on [0,1]"; + if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; + if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; this->motorName = motorName; this->controllerID = controllerID; From 6f351723d5f15e31eabc8f80df01059c40faaab8 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 16:34:51 -0500 Subject: [PATCH 23/85] Changed ArmMotor rad->enc and enc->rad methods to accurately compute the radians or encoders on their proper bounds, moved the two methods closer for readability. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index b0f449c8..afcd0880 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -5,7 +5,11 @@ static const unsigned long int COUNTS_PER_ROTATION = UINT_FAST32_MAX; int ArmMotor::radToEnc(float rads){ - return COUNTS_PER_ROTATION * rads / (2 * M_PI); + return COUNTS_PER_ROTATION * (rads + M_PI) / (2 * M_PI); +} + +double ArmMotor::getRads(){ + return this->getEncoderCounts() * 2 * M_PI / COUNTS_PER_ROTATION - M_PI; } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ @@ -83,10 +87,6 @@ void ArmMotor::runToTarget(double rads, float power){ runToTarget(this->radToEnc(rads), power, false); } -double ArmMotor::getRads(){ - return this->getEncoderCounts() * 2 * M_PI / COUNTS_PER_ROTATION; -} - std::string ArmMotor::getMotorName(){ return this->motorName; } From 2c23e34c596ad52bb7a72432580c8702593be821 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 18:13:22 -0500 Subject: [PATCH 24/85] Updated encoder math to reflect the boundries of the encoders and use the right datatypes to encapsulate the values; made the encoder bounds and COUNTS_PER_ROTATION into static constant variables. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 30 +++++++++++++++-------- src/wr_control_drive_arm/src/ArmMotor.hpp | 15 +++++++----- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index afcd0880..5125c3e8 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -2,14 +2,15 @@ #define Std_UInt32 std_msgs::UInt32::ConstPtr& -static const unsigned long int COUNTS_PER_ROTATION = UINT_FAST32_MAX; +unsigned int const ArmMotor::COUNTS_PER_ROTATION = UINT_FAST32_MAX; +unsigned int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; -int ArmMotor::radToEnc(float rads){ - return COUNTS_PER_ROTATION * (rads + M_PI) / (2 * M_PI); +unsigned int ArmMotor::radToEnc(float rads){ + return ArmMotor::COUNTS_PER_ROTATION * (rads + M_PI) / (2 * M_PI); } double ArmMotor::getRads(){ - return this->getEncoderCounts() * 2 * M_PI / COUNTS_PER_ROTATION - M_PI; + return this->getEncoderCounts() * 2 * M_PI / ArmMotor::COUNTS_PER_ROTATION - M_PI; } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ @@ -39,18 +40,27 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in // ~ArmMotor(); -int ArmMotor::getEncoderCounts(){ +unsigned int ArmMotor::getEncoderCounts(){ return this->encoderVal; } // void resetEncoder(); -void ArmMotor::runToTarget(int targetCounts, float power){ +void ArmMotor::runToTarget(unsigned int targetCounts, float power){ this->runToTarget(targetCounts, power, false); } -bool ArmMotor::hasReachedTarget(int targetCounts){ - return abs(targetCounts - this->getEncoderCounts()) < 10; +bool ArmMotor::hasReachedTarget(unsigned int targetCounts, unsigned int tolerance){ + unsigned int lBound = ((targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1] < 0 ? 2 * targetCounts - tolerance : targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1]; + unsigned int uBound = (targetCounts + tolerance) % ArmMotor::ENCODER_BOUNDS[1]; + if(lBound < uBound) + return this->getEncoderCounts() <= uBound && this->getEncoderCounts() >=lBound; + else + return this->getEncoderCounts() <= uBound || this->getEncoderCounts() >=lBound; +} + +bool ArmMotor::hasReachedTarget(unsigned int targetCounts){ + return ArmMotor::hasReachedTarget(targetCounts, 10); } MotorState ArmMotor::getMotorState(){ @@ -66,9 +76,9 @@ void ArmMotor::setPower(float power){ this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; } -void ArmMotor::runToTarget(int targetCounts, float power, bool block){ +void ArmMotor::runToTarget(unsigned int targetCounts, float power, bool block){ if(this->getMotorState() != MotorState::RUN_TO_TARGET){ - power = abs(power) * abs(targetCounts - this->getEncoderCounts())/(targetCounts - this->getEncoderCounts()); + power = abs(power) * abs(((long)targetCounts - this->getEncoderCounts()))/(targetCounts - this->getEncoderCounts()); this->setPower(power); this->currState = MotorState::RUN_TO_TARGET; } diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 58acccf1..d6b94afe 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -10,29 +10,32 @@ enum MotorState{ }; class ArmMotor{ private: + static unsigned int const COUNTS_PER_ROTATION; + static unsigned int const ENCODER_BOUNDS[2]; MotorState currState; std::string motorName; unsigned int controllerID; unsigned int motorID; - unsigned long int encoderVal; + unsigned int encoderVal; ros::Subscriber encRead; ros::Publisher speedPub; std_msgs::Int16 *powerMsg; - static int radToEnc(float rad); + static unsigned int radToEnc(float rad); void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); public: ArmMotor(); ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); // ~ArmMotor(); - int getEncoderCounts(); + unsigned int getEncoderCounts(); // void resetEncoder(); - void runToTarget(int targetCounts, float power); - void runToTarget(int targetCounts, float power, bool block); + void runToTarget(unsigned int targetCounts, float power); + void runToTarget(unsigned int targetCounts, float power, bool block); void runToTarget(double rads, float power); MotorState getMotorState(); void setPower(float power); double getRads(); std::string getMotorName(); - bool hasReachedTarget(int targetCounts); + bool hasReachedTarget(unsigned int targetCounts); + bool hasReachedTarget(unsigned int targetCounts, unsigned int tolerance); float getPower(); }; \ No newline at end of file From c7f7c5398ef81e1dda32ef1a1e4854fbea679201 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 18:29:26 -0500 Subject: [PATCH 25/85] Updated encoder datatypes to reflect C++ standard for data size requirement. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 27 ++++++++++++----------- src/wr_control_drive_arm/src/ArmMotor.hpp | 24 ++++++++++---------- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 5125c3e8..10f4179b 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -2,10 +2,10 @@ #define Std_UInt32 std_msgs::UInt32::ConstPtr& -unsigned int const ArmMotor::COUNTS_PER_ROTATION = UINT_FAST32_MAX; -unsigned int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; +unsigned long int const ArmMotor::COUNTS_PER_ROTATION = UINT_FAST32_MAX; +unsigned long int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; -unsigned int ArmMotor::radToEnc(float rads){ +unsigned long int ArmMotor::radToEnc(float rads){ return ArmMotor::COUNTS_PER_ROTATION * (rads + M_PI) / (2 * M_PI); } @@ -23,7 +23,7 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ ArmMotor::ArmMotor(){} -ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n){ +ArmMotor::ArmMotor(std::string motorName, unsigned long int controllerID, unsigned long int motorID, ros::NodeHandle* n){ if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; @@ -40,26 +40,26 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in // ~ArmMotor(); -unsigned int ArmMotor::getEncoderCounts(){ +unsigned long int ArmMotor::getEncoderCounts(){ return this->encoderVal; } // void resetEncoder(); -void ArmMotor::runToTarget(unsigned int targetCounts, float power){ +void ArmMotor::runToTarget(unsigned long int targetCounts, float power){ this->runToTarget(targetCounts, power, false); } -bool ArmMotor::hasReachedTarget(unsigned int targetCounts, unsigned int tolerance){ - unsigned int lBound = ((targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1] < 0 ? 2 * targetCounts - tolerance : targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1]; - unsigned int uBound = (targetCounts + tolerance) % ArmMotor::ENCODER_BOUNDS[1]; +bool ArmMotor::hasReachedTarget(unsigned long int targetCounts, unsigned long int tolerance){ + unsigned long int lBound = ((targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1] < 0 ? 2 * targetCounts - tolerance : targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1]; + unsigned long int uBound = (targetCounts + tolerance) % ArmMotor::ENCODER_BOUNDS[1]; if(lBound < uBound) return this->getEncoderCounts() <= uBound && this->getEncoderCounts() >=lBound; else return this->getEncoderCounts() <= uBound || this->getEncoderCounts() >=lBound; } -bool ArmMotor::hasReachedTarget(unsigned int targetCounts){ +bool ArmMotor::hasReachedTarget(unsigned long int targetCounts){ return ArmMotor::hasReachedTarget(targetCounts, 10); } @@ -76,9 +76,10 @@ void ArmMotor::setPower(float power){ this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; } -void ArmMotor::runToTarget(unsigned int targetCounts, float power, bool block){ - if(this->getMotorState() != MotorState::RUN_TO_TARGET){ - power = abs(power) * abs(((long)targetCounts - this->getEncoderCounts()))/(targetCounts - this->getEncoderCounts()); +void ArmMotor::runToTarget(unsigned long int targetCounts, float power, bool block){ + if(this->getMotorState() != MotorState::RUN_TO_TARGET && !this->hasReachedTarget(targetCounts)){ + unsigned long int direction = targetCounts - this->getEncoderCounts(); + power = abs(power) * ((direction > 0) - (direction < 0)); this->setPower(power); this->currState = MotorState::RUN_TO_TARGET; } diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index d6b94afe..1148845e 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -10,32 +10,32 @@ enum MotorState{ }; class ArmMotor{ private: - static unsigned int const COUNTS_PER_ROTATION; - static unsigned int const ENCODER_BOUNDS[2]; + static unsigned long int const COUNTS_PER_ROTATION; + static unsigned long int const ENCODER_BOUNDS[2]; MotorState currState; std::string motorName; - unsigned int controllerID; - unsigned int motorID; - unsigned int encoderVal; + unsigned long int controllerID; + unsigned long int motorID; + unsigned long int encoderVal; ros::Subscriber encRead; ros::Publisher speedPub; std_msgs::Int16 *powerMsg; - static unsigned int radToEnc(float rad); + static unsigned long int radToEnc(float rad); void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); public: ArmMotor(); - ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); + ArmMotor(std::string motorName, unsigned long int controllerID, unsigned long int motorID, ros::NodeHandle* n); // ~ArmMotor(); - unsigned int getEncoderCounts(); + unsigned long int getEncoderCounts(); // void resetEncoder(); - void runToTarget(unsigned int targetCounts, float power); - void runToTarget(unsigned int targetCounts, float power, bool block); + void runToTarget(unsigned long int targetCounts, float power); + void runToTarget(unsigned long int targetCounts, float power, bool block); void runToTarget(double rads, float power); MotorState getMotorState(); void setPower(float power); double getRads(); std::string getMotorName(); - bool hasReachedTarget(unsigned int targetCounts); - bool hasReachedTarget(unsigned int targetCounts, unsigned int tolerance); + bool hasReachedTarget(unsigned long int targetCounts); + bool hasReachedTarget(unsigned long int targetCounts, unsigned long int tolerance); float getPower(); }; \ No newline at end of file From c28bcfbc6fde090667a04eb397da7faa4e1e13d7 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 19:51:42 -0500 Subject: [PATCH 26/85] Added test launch script for the arm control stack. --- test_arm_control_stack.sh | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100755 test_arm_control_stack.sh diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh new file mode 100755 index 00000000..2030808c --- /dev/null +++ b/test_arm_control_stack.sh @@ -0,0 +1,16 @@ +#!/bin/bash +ROOT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +cd $ROOT_DIR +catkin_make +gnome-terminal -- roscore +rostopic list &>./out.temp; +COUNT=$(grep -Ec "Unable to communicate with master" out.temp) +while [ $COUNT -ne 0 ]; do + rostopic list &>./out.temp; + COUNT=$(grep -Ec "Unable to communicate with master" out.temp) +done +rm ./out.temp; +export WROVER_LOCAL=true +gnome-terminal -- roslaunch wr_entry_point test_arm.launch +gnome-terminal -- rosrun wr_control_drive_arm ArmControlSystem +gnome-terminal -- roslaunch wroboarm_21 demo_test.launch From 291db4f27a34cc36b21b7cf51bcb08c3b3e4e65d Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 19:58:33 -0500 Subject: [PATCH 27/85] Removed noisy debugging code from ArmControlSystem.cpp; errors not removed with it. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index b02cc699..75b052e5 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -18,7 +18,6 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server bool hasPositionFinished = false; std::vector names; std::vector positions; - unsigned long long int i2 = 0; while(!hasPositionFinished){ bool temp = true; sensor_msgs::JointState js_msg; @@ -26,13 +25,6 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server names.clear(); positions.clear(); - if(i2%10000==0){ - std::cout<<"Motor0 Target (rads):\t"<getEncoderCounts()<getRads()<runToTarget(currTargetPosition.positions[j], 0.1); temp &= motors[j]->getMotorState() == MotorState::STOP; From 03398e0b25b55dad54593690a3c84235bf474420 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 21:17:37 -0500 Subject: [PATCH 28/85] Modified the encoder/radian transformation to actually modify the actual transformation desired in accordance with the tests here: https://www.desmos.com/calculator/j8zidhiefy --- src/wr_control_drive_arm/src/ArmMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 10f4179b..d3c0ee33 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -6,11 +6,11 @@ unsigned long int const ArmMotor::COUNTS_PER_ROTATION = UINT_FAST32_MAX; unsigned long int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; unsigned long int ArmMotor::radToEnc(float rads){ - return ArmMotor::COUNTS_PER_ROTATION * (rads + M_PI) / (2 * M_PI); + return ((unsigned long int)ArmMotor::COUNTS_PER_ROTATION*fmod(rads,2 * M_PI)/(2 * M_PI)); } double ArmMotor::getRads(){ - return this->getEncoderCounts() * 2 * M_PI / ArmMotor::COUNTS_PER_ROTATION - M_PI; + return fmod(this->getEncoderCounts() / ArmMotor::COUNTS_PER_ROTATION * 2 * M_PI - M_PI, 2 * M_PI); } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ From cc7b8c72463c84b0efee4cf7f3ec49d0a4eb15d3 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 22:05:04 -0500 Subject: [PATCH 29/85] Modified the formulas again due to C++ negative modulus behaviour. Added some moderately noisy but tolerable debugging information. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 5 ++++- src/wr_control_drive_arm/src/ArmMotor.cpp | 6 +++--- src/wr_control_drive_arm/src/ArmMotor.hpp | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 75b052e5..18dbbf76 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -26,11 +26,14 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server positions.clear(); for(int j = 0; j < currTargetPosition.positions.size(); j++){ - motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); + motors[j]->runToTarget(currTargetPosition.positions[j], 0.5); temp &= motors[j]->getMotorState() == MotorState::STOP; names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); + std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getEncoderCounts() / ArmMotor::COUNTS_PER_ROTATION * 2 * M_PI - M_PI, 2 * M_PI); +float ArmMotor::getRads(){ + return fmod((this->getEncoderCounts()) / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 1148845e..cd83a348 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -33,7 +33,7 @@ class ArmMotor{ void runToTarget(double rads, float power); MotorState getMotorState(); void setPower(float power); - double getRads(); + float getRads(); std::string getMotorName(); bool hasReachedTarget(unsigned long int targetCounts); bool hasReachedTarget(unsigned long int targetCounts, unsigned long int tolerance); From 7c90f71356f947aeacc56924927c33bff7d67db6 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 1 Apr 2021 22:18:26 -0500 Subject: [PATCH 30/85] Added a sleep rate for realism for the actual robot, amped the speed of the arm for testing purposes. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 18dbbf76..df5dd64d 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -18,6 +18,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server bool hasPositionFinished = false; std::vector names; std::vector positions; + ros::Rate loop(50); while(!hasPositionFinished){ bool temp = true; sensor_msgs::JointState js_msg; @@ -26,7 +27,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server positions.clear(); for(int j = 0; j < currTargetPosition.positions.size(); j++){ - motors[j]->runToTarget(currTargetPosition.positions[j], 0.5); + motors[j]->runToTarget(currTargetPosition.positions[j], 1); temp &= motors[j]->getMotorState() == MotorState::STOP; names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); @@ -40,6 +41,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server jointStatePublisher.publish(js_msg); hasPositionFinished = temp; + loop.sleep(); } } as->setSucceeded(); From a6feb10d5f5c710102d7feae6f5d65e867d6c080 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 17:18:38 -0500 Subject: [PATCH 31/85] Improved readability of ArmControlSystem.cpp. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index df5dd64d..92a170ab 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -31,6 +31,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server temp &= motors[j]->getMotorState() == MotorState::STOP; names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); + std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()< Date: Sun, 4 Apr 2021 17:27:07 -0500 Subject: [PATCH 32/85] Added an Euclidean-correct modulus function for mathematical consistency and updated the rad<->enc conversion functions to be consistent with https://www.desmos.com/calculator/nwxtenccc6. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 16 +++++++++++----- src/wr_control_drive_arm/src/ArmMotor.hpp | 3 ++- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index d5015303..5524d735 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -5,12 +5,18 @@ unsigned long int const ArmMotor::COUNTS_PER_ROTATION = UINT_FAST32_MAX; unsigned long int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; -unsigned long int ArmMotor::radToEnc(float rads){ - return ArmMotor::COUNTS_PER_ROTATION*fmod(rads,2 * M_PI)/(2 * M_PI); +template T ArmMotor::corrMod(T i, T j){ + int dir = -((i-j>0)-(i-j<0)); + while(i<0||i>=j)i+=dir*j; + return i; +} + +unsigned long int ArmMotor::radToEnc(double rads){ + return ArmMotor::COUNTS_PER_ROTATION*ArmMotor::corrMod(rads,2 * M_PI)/(2 * M_PI); } float ArmMotor::getRads(){ - return fmod((this->getEncoderCounts()) / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; + return ArmMotor::corrMod(this->getEncoderCounts() / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ @@ -78,8 +84,8 @@ void ArmMotor::setPower(float power){ void ArmMotor::runToTarget(unsigned long int targetCounts, float power, bool block){ if(this->getMotorState() != MotorState::RUN_TO_TARGET && !this->hasReachedTarget(targetCounts)){ - unsigned long int direction = targetCounts - this->getEncoderCounts(); - power = abs(power) * ((direction > 0) - (direction < 0)); + long long int direction = targetCounts - this->getEncoderCounts(); + power = abs(power) * (corrMod(direction, ((long long int)ArmMotor::COUNTS_PER_ROTATION)) < corrMod(-direction, ((long long int)ArmMotor::COUNTS_PER_ROTATION)) ? 1 : -1); this->setPower(power); this->currState = MotorState::RUN_TO_TARGET; } diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index cd83a348..765aa711 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -20,8 +20,9 @@ class ArmMotor{ ros::Subscriber encRead; ros::Publisher speedPub; std_msgs::Int16 *powerMsg; - static unsigned long int radToEnc(float rad); + static unsigned long int radToEnc(double rad); void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); + template static T corrMod(T i, T j); public: ArmMotor(); ArmMotor(std::string motorName, unsigned long int controllerID, unsigned long int motorID, ros::NodeHandle* n); From 070a00230e58f3ac1d1df39b0448fe7085d30ace Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 17:28:16 -0500 Subject: [PATCH 33/85] Updated `COUNTS_PER_ROTATION` to be the actual UINT32 limit; apparently UINT_FAST32 is something else. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 5524d735..e3a9d933 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -2,7 +2,7 @@ #define Std_UInt32 std_msgs::UInt32::ConstPtr& -unsigned long int const ArmMotor::COUNTS_PER_ROTATION = UINT_FAST32_MAX; +unsigned long int const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; unsigned long int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; template T ArmMotor::corrMod(T i, T j){ From 958a302f1e6384f57a30ce61ea042e35bba00953 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 17:32:17 -0500 Subject: [PATCH 34/85] =?UTF-8?q?Updated=20the=20default=20encoder=20toler?= =?UTF-8?q?ance=20to=20statically=20reflect=20=C2=B10.1=20degree=20toleran?= =?UTF-8?q?ce=20w.r.t.=20the=20`COUNTS=5FPER=5FROTATION`.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/wr_control_drive_arm/src/ArmMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index e3a9d933..147dd4b5 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -66,7 +66,7 @@ bool ArmMotor::hasReachedTarget(unsigned long int targetCounts, unsigned long in } bool ArmMotor::hasReachedTarget(unsigned long int targetCounts){ - return ArmMotor::hasReachedTarget(targetCounts, 10); + return ArmMotor::hasReachedTarget(targetCounts, 1200000); } MotorState ArmMotor::getMotorState(){ From 4fa4331236d14327db22b1fe5857cb3cf23d543e Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 17:35:06 -0500 Subject: [PATCH 35/85] Added the `time_warp` parameter to the mock launch file in anticipation of WisconsinRobotics/wroboclaw#2. --- src/wr_hsi_roboclaw/launch/roboclaw_mock.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch b/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch index 396b5b53..17627373 100644 --- a/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch +++ b/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch @@ -8,6 +8,7 @@ + From 590e847541fcd044e318fa0ec15e45276555ec89 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 17:40:18 -0500 Subject: [PATCH 36/85] Updated the demo_test.launch file to disable trajectory execution timeouts. This could be replaced in the future with the multiplier parameter `move_group/trajectory_execution/allowed_execution_duration_scaling` that extends the execution timout by a scalar multiplier or `move_group/trajectory_execution/allowed_goal_duration_margin` for adding a fixed time tolerance to motions. --- src/wroboarm_21/launch/demo_test.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/src/wroboarm_21/launch/demo_test.launch b/src/wroboarm_21/launch/demo_test.launch index 5d722af0..a69407ab 100644 --- a/src/wroboarm_21/launch/demo_test.launch +++ b/src/wroboarm_21/launch/demo_test.launch @@ -42,6 +42,7 @@ + From e052d32889cee24868bde01539ed46168e98de3a Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 19:15:48 -0500 Subject: [PATCH 37/85] Updated the Arm Control stack test script to re-navigate back to the original directory, regarless of where the script was called. --- test_arm_control_stack.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index 2030808c..f4299b44 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -14,3 +14,4 @@ export WROVER_LOCAL=true gnome-terminal -- roslaunch wr_entry_point test_arm.launch gnome-terminal -- rosrun wr_control_drive_arm ArmControlSystem gnome-terminal -- roslaunch wroboarm_21 demo_test.launch +cd - From 0bea77a46328b27f3f99153ecc336c60052c4dfa Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 19:19:52 -0500 Subject: [PATCH 38/85] Updated encoder data types to reflect the amount of data stored in them (x64 processors default to 32 bits/integer). --- src/wr_control_drive_arm/src/ArmMotor.cpp | 26 +++++++++++------------ src/wr_control_drive_arm/src/ArmMotor.hpp | 24 ++++++++++----------- 2 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 147dd4b5..fd9c7059 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -2,8 +2,8 @@ #define Std_UInt32 std_msgs::UInt32::ConstPtr& -unsigned long int const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; -unsigned long int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; +unsigned int const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; +unsigned int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; template T ArmMotor::corrMod(T i, T j){ int dir = -((i-j>0)-(i-j<0)); @@ -11,7 +11,7 @@ template T ArmMotor::corrMod(T i, T j){ return i; } -unsigned long int ArmMotor::radToEnc(double rads){ +unsigned int ArmMotor::radToEnc(double rads){ return ArmMotor::COUNTS_PER_ROTATION*ArmMotor::corrMod(rads,2 * M_PI)/(2 * M_PI); } @@ -29,7 +29,7 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ ArmMotor::ArmMotor(){} -ArmMotor::ArmMotor(std::string motorName, unsigned long int controllerID, unsigned long int motorID, ros::NodeHandle* n){ +ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n){ if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; @@ -46,26 +46,26 @@ ArmMotor::ArmMotor(std::string motorName, unsigned long int controllerID, unsign // ~ArmMotor(); -unsigned long int ArmMotor::getEncoderCounts(){ +unsigned int ArmMotor::getEncoderCounts(){ return this->encoderVal; } // void resetEncoder(); -void ArmMotor::runToTarget(unsigned long int targetCounts, float power){ +void ArmMotor::runToTarget(unsigned int targetCounts, float power){ this->runToTarget(targetCounts, power, false); } -bool ArmMotor::hasReachedTarget(unsigned long int targetCounts, unsigned long int tolerance){ - unsigned long int lBound = ((targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1] < 0 ? 2 * targetCounts - tolerance : targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1]; - unsigned long int uBound = (targetCounts + tolerance) % ArmMotor::ENCODER_BOUNDS[1]; +bool ArmMotor::hasReachedTarget(unsigned int targetCounts, unsigned int tolerance){ + unsigned int lBound = ((targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1] < 0 ? 2 * targetCounts - tolerance : targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1]; + unsigned int uBound = (targetCounts + tolerance) % ArmMotor::ENCODER_BOUNDS[1]; if(lBound < uBound) return this->getEncoderCounts() <= uBound && this->getEncoderCounts() >=lBound; else return this->getEncoderCounts() <= uBound || this->getEncoderCounts() >=lBound; } -bool ArmMotor::hasReachedTarget(unsigned long int targetCounts){ +bool ArmMotor::hasReachedTarget(unsigned int targetCounts){ return ArmMotor::hasReachedTarget(targetCounts, 1200000); } @@ -82,10 +82,10 @@ void ArmMotor::setPower(float power){ this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; } -void ArmMotor::runToTarget(unsigned long int targetCounts, float power, bool block){ +void ArmMotor::runToTarget(unsigned int targetCounts, float power, bool block){ if(this->getMotorState() != MotorState::RUN_TO_TARGET && !this->hasReachedTarget(targetCounts)){ - long long int direction = targetCounts - this->getEncoderCounts(); - power = abs(power) * (corrMod(direction, ((long long int)ArmMotor::COUNTS_PER_ROTATION)) < corrMod(-direction, ((long long int)ArmMotor::COUNTS_PER_ROTATION)) ? 1 : -1); + long int direction = targetCounts - this->getEncoderCounts(); + power = abs(power) * (corrMod(direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) ? 1 : -1); this->setPower(power); this->currState = MotorState::RUN_TO_TARGET; } diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 765aa711..c2c10e88 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -10,33 +10,33 @@ enum MotorState{ }; class ArmMotor{ private: - static unsigned long int const COUNTS_PER_ROTATION; - static unsigned long int const ENCODER_BOUNDS[2]; + static unsigned int const COUNTS_PER_ROTATION; + static unsigned int const ENCODER_BOUNDS[2]; MotorState currState; std::string motorName; - unsigned long int controllerID; - unsigned long int motorID; - unsigned long int encoderVal; + unsigned int controllerID; + unsigned int motorID; + unsigned int encoderVal; ros::Subscriber encRead; ros::Publisher speedPub; std_msgs::Int16 *powerMsg; - static unsigned long int radToEnc(double rad); + static unsigned int radToEnc(double rad); void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); template static T corrMod(T i, T j); public: ArmMotor(); - ArmMotor(std::string motorName, unsigned long int controllerID, unsigned long int motorID, ros::NodeHandle* n); + ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); // ~ArmMotor(); - unsigned long int getEncoderCounts(); + unsigned int getEncoderCounts(); // void resetEncoder(); - void runToTarget(unsigned long int targetCounts, float power); - void runToTarget(unsigned long int targetCounts, float power, bool block); + void runToTarget(unsigned int targetCounts, float power); + void runToTarget(unsigned int targetCounts, float power, bool block); void runToTarget(double rads, float power); MotorState getMotorState(); void setPower(float power); float getRads(); std::string getMotorName(); - bool hasReachedTarget(unsigned long int targetCounts); - bool hasReachedTarget(unsigned long int targetCounts, unsigned long int tolerance); + bool hasReachedTarget(unsigned int targetCounts); + bool hasReachedTarget(unsigned int targetCounts, unsigned int tolerance); float getPower(); }; \ No newline at end of file From abf59d644b7b584330bf80f58195eaeceb7f15fb Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sun, 4 Apr 2021 22:50:31 -0500 Subject: [PATCH 39/85] Updated the roboclaw_mock launch file and the roboclaw_enc config file to be prepared for the new implementation of WisconsinRobotics/wroboclaw#2. --- src/wr_hsi_roboclaw/config/roboclaw_enc.yaml | 32 ++++++++++++++----- .../launch/roboclaw_mock.launch | 3 +- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml b/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml index 5c73563d..11268a6f 100644 --- a/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml +++ b/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml @@ -4,23 +4,39 @@ claws: aux0: # arm turntable/shoulder, science ? topic: '/hsi/roboclaw/aux0' address: 0x80 - enc_left: true # TODO probably don't actually need all these encoders - enc_right: true + enc_left: + enabled: true + max_speed: 300000000 # TODO probably don't actually need all these encoders + enc_right: + enabled: true + max_speed: 300000000 aux1: # arm elbow/forearm, science ? topic: '/hsi/roboclaw/aux1' address: 0x81 - enc_left: true - enc_right: true + enc_left: + enabled: true + max_speed: 300000000 + enc_right: + enabled: true + max_speed: 300000000 aux2: # arm wrist, science ? topic: '/hsi/roboclaw/aux2' address: 0x82 - enc_left: true - enc_right: true + enc_left: + enabled: true + max_speed: 300000000 + enc_right: + enabled: true + max_speed: 300000000 aux3: # arm manip, science ? topic: '/hsi/roboclaw/aux3' address: 0x83 - enc_left: true - enc_right: true + enc_left: + enabled: true + max_speed: 300000000 + enc_right: + enabled: true + max_speed: 300000000 drive: # drive train topic: '/hsi/roboclaw/drive' address: 0x84 diff --git a/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch b/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch index 17627373..6fea09ed 100644 --- a/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch +++ b/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch @@ -1,6 +1,6 @@ - + @@ -8,7 +8,6 @@ - From 058be8de0228238d51737e76bafcd78bf0e35808 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 10:59:17 -0500 Subject: [PATCH 40/85] Started documentation, removed unneccesary checks from the code. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 7 +++---- src/wr_control_drive_arm/src/ArmMotor.cpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 92a170ab..c2a3349c 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -9,11 +9,10 @@ ros::Publisher jointStatePublisher; typedef actionlib::SimpleActionServer Server; void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { + // For each point in the trajectory execution sequence... for(int i = 0; i < goal->trajectory.points.size(); i++){ + // Capture the current goal for easy reference trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; - for(int j = 0; j < currTargetPosition.positions.size(); j++){ - motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); - } bool hasPositionFinished = false; std::vector names; @@ -27,7 +26,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server positions.clear(); for(int j = 0; j < currTargetPosition.positions.size(); j++){ - motors[j]->runToTarget(currTargetPosition.positions[j], 1); + motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); temp &= motors[j]->getMotorState() == MotorState::STOP; names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index fd9c7059..f152e215 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -83,7 +83,7 @@ void ArmMotor::setPower(float power){ } void ArmMotor::runToTarget(unsigned int targetCounts, float power, bool block){ - if(this->getMotorState() != MotorState::RUN_TO_TARGET && !this->hasReachedTarget(targetCounts)){ + if(!this->hasReachedTarget(targetCounts)){ long int direction = targetCounts - this->getEncoderCounts(); power = abs(power) * (corrMod(direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) ? 1 : -1); this->setPower(power); From 439da513db3becaafb23803940018c2400654316 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 11:07:25 -0500 Subject: [PATCH 41/85] Continued documentation, removed use of `temp` variable to track if the current position is completed as the value can be stored in the existing variable already. --- .../src/ArmControlSystem.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index c2a3349c..c2fcf6e6 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -4,22 +4,38 @@ #include #include "ArmMotor.hpp" +// Define space for 6 ArmMotors ArmMotor *motors[6]; +// Define the Joint State Data Publisher ros::Publisher jointStatePublisher; +// Simplify the SimpleActionServer typename typedef actionlib::SimpleActionServer Server; +/** + * @brief Perform the given action as interpreted as moving the arm joints to specified positions + * + * @param goal The goal state given + * @param as The Action Server this is occuring on + */ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { // For each point in the trajectory execution sequence... for(int i = 0; i < goal->trajectory.points.size(); i++){ // Capture the current goal for easy reference trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; + // Track whether or not the current position is done bool hasPositionFinished = false; + // Capture the names of the motors std::vector names; + // Capture the positions of the motors std::vector positions; + // Keep max loop rate at 50 Hz ros::Rate loop(50); + + // While the current position is not complete yet... while(!hasPositionFinished){ - bool temp = true; + // Assume the current action is done until proven otherwise + hasPositionFinished = true; sensor_msgs::JointState js_msg; names.clear(); @@ -27,7 +43,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server for(int j = 0; j < currTargetPosition.positions.size(); j++){ motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); - temp &= motors[j]->getMotorState() == MotorState::STOP; + hasPositionFinished &= motors[j]->getMotorState() == MotorState::STOP; names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); @@ -40,7 +56,6 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server js_msg.position = positions; jointStatePublisher.publish(js_msg); - hasPositionFinished = temp; loop.sleep(); } } From 03fdde1acb37c8418197f075e8eeeb03d201917d Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 11:15:09 -0500 Subject: [PATCH 42/85] Continued documentation, added `TODO`s for future work. --- .../src/ArmControlSystem.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index c2fcf6e6..517d1105 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -36,29 +36,44 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server while(!hasPositionFinished){ // Assume the current action is done until proven otherwise hasPositionFinished = true; + // Create the Joint State message for the current update cycle sensor_msgs::JointState js_msg; + // Clear the list of motor names and position data names.clear(); positions.clear(); + // For each motor specified in the currTargetPosition... for(int j = 0; j < currTargetPosition.positions.size(); j++){ + // Each motor should run to its respective target position at a fixed speed + // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); + // The position has only finished if every motor is STOPped hasPositionFinished &= motors[j]->getMotorState() == MotorState::STOP; + // Push the current motor name and position data to the Joint State data tracking list names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); + // DEBUGGING OUTPUT: Print each motor's name, radian position, and encoder position std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<setSucceeded(); } From 8b99b6aaa979d365008d9251242c7e5616ae4ecd Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 11:19:37 -0500 Subject: [PATCH 43/85] Continued documentation. --- .../src/ArmControlSystem.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 517d1105..9afe9ccf 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -77,12 +77,21 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server as->setSucceeded(); } - +/** + * @brief The main executable method of the node. Starts the ROS node and the Action Server for processing Arm Control commands + * + * @param argc The number of program arguments + * @param argv The given program arguments + * @return int The status code on exiting the program + */ int main(int argc, char** argv) { + // Initialize the current node as ArmControlSystem ros::init(argc, argv, "ArmControlSystem"); + // Create the NodeHandle to the current ROS node ros::NodeHandle n; + // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node motors[0] = new ArmMotor("link1_joint", 0, 0, &n); motors[1] = new ArmMotor("link2_joint", 0, 1, &n); motors[2] = new ArmMotor("link3_joint", 1, 0, &n); @@ -90,10 +99,16 @@ int main(int argc, char** argv) motors[4] = new ArmMotor("link5_joint", 2, 0, &n); motors[5] = new ArmMotor("link6_joint", 2, 1, &n); + // Initialize the Joint State Data Publisher jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); - Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); + // Initialize the Action Server + Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); + // Start the Action Server server.start(); + + // ROS spin for communication with other nodes ros::spin(); + // Return 0 on exit (successful exit) return 0; } \ No newline at end of file From bcaae15154e9a5aa5622cb57c07e956826217a4e Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 12:19:54 -0500 Subject: [PATCH 44/85] Updated documentation to be doxygen-compatible. --- .../src/ArmControlSystem.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 9afe9ccf..ccf6d74c 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -1,14 +1,26 @@ +/** + * @file ArmControlSystem.cpp + * @author Ben Nowotny + * @brief The exeutable file to run the Arm Control Action Server + * @date 2021-04-05 + */ #include "ros/ros.h" #include #include #include #include "ArmMotor.hpp" -// Define space for 6 ArmMotors +/** + * @brief Defines space for all ArmMotor.hpp references + */ ArmMotor *motors[6]; -// Define the Joint State Data Publisher +/** + * @brief The Joint State Publisher for MoveIt + */ ros::Publisher jointStatePublisher; -// Simplify the SimpleActionServer typename +/** + * @brief Simplify the SimpleActionServer reference name + */ typedef actionlib::SimpleActionServer Server; /** From a549b1c92024b182f05bf5beae8e0aedccb53a8c Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 16:15:37 -0500 Subject: [PATCH 45/85] Updated Doxygen link. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index ccf6d74c..7eda7425 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -11,7 +11,7 @@ #include "ArmMotor.hpp" /** - * @brief Defines space for all ArmMotor.hpp references + * @brief Defines space for all ArmMotor references */ ArmMotor *motors[6]; /** From 31df29132b57a0e330aeffe7f75aea888e181e12 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 19:13:40 -0500 Subject: [PATCH 46/85] Added ArmMotor.hpp doxygen documentation, removed unneeded methods. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 9 +- src/wr_control_drive_arm/src/ArmMotor.hpp | 134 +++++++++++++++++++++- 2 files changed, 135 insertions(+), 8 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index f152e215..70c5e2dd 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -1,3 +1,9 @@ +/** + * @file ArmMotor.cpp + * @author Ben Nowotny + * @brief The implementation of the ArmMotor class + * @date 2021-04-05 + */ #include "ArmMotor.hpp" #define Std_UInt32 std_msgs::UInt32::ConstPtr& @@ -26,9 +32,6 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ // std::cout<<"value in method: "<getEncoderCounts()< 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index c2c10e88..5ad74f23 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -1,3 +1,9 @@ +/** + * @file ArmMotor.hpp + * @author Ben Nowotny + * @brief ablskjlfkejfs + * @date 2021-04-05 + */ #include #include "ros/ros.h" #include "std_msgs/UInt32.h" @@ -5,38 +11,156 @@ #include "math.h" #include -enum MotorState{ - STOP, MOVING, RUN_TO_TARGET +/** + * @brief An enumeration of states for a motor to be in. + */ +enum class MotorState{ + /// A Motor is stopped (not moving, 0 power command) + STOP, + /// A Motor is moving (non-0 power command) + MOVING, + /// A Motor is running to a given target + RUN_TO_TARGET }; +/** + * @brief A way to control arm motors with WRoboclaw + */ class ArmMotor{ private: + /// The number of encoder counts per rotation static unsigned int const COUNTS_PER_ROTATION; + /// The upper and lower bounds of encoder rotation for absolute encoders (index 0 is lower, index 1 is upper) static unsigned int const ENCODER_BOUNDS[2]; + /// The current state of the motor MotorState currState; + /// The joint name of the current motor std::string motorName; + /// The ID of the WRoboclaw controller unsigned int controllerID; + /// The ID of the motor within the WRoboclaw controller unsigned int motorID; + /// The current encoder value unsigned int encoderVal; + /// The ROS Subscriber that reads the encoders ros::Subscriber encRead; + /// The ROS Publisher that publishes motor speed commands ros::Publisher speedPub; + /// A pointer to the most recent power message sent std_msgs::Int16 *powerMsg; + + /** + * @brief A static conversion from radians to encoder counts + * + * @param rad The input number of radians + * @return unsigned int The corresponding encoder count bounded by ENCODER_BOUNDS + */ static unsigned int radToEnc(double rad); + + /** + * @brief Subscriber callback for encRead, captured the encoder value of the current motor + * + * @param msg The encoder value message as captured by encRead + */ void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); + + /** + * @brief Performs Euclidean correct modulus between two inputs of the same type + * + * @tparam T Some type on which addition and comparisons occur + * @param i The dividend of the modulus + * @param j The divisor of the modulus + * @return T The Euclidean-correct remainder bounded on [0, j) + */ template static T corrMod(T i, T j); + public: - ArmMotor(); + /** + * @brief Constructs a new ArmMotor + * + * @param motorName The joint name of the motor + * @param controllerID The WRoboclaw controller ID for this motor + * @param motorID The motor ID within its WRoboclaw controller + * @param n A NodeHandle reference to the constructing Node + */ ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); - // ~ArmMotor(); + + /** + * @brief Gets the encoder value of the motor + * + * @return unsigned int The current encoder value of the motor + */ unsigned int getEncoderCounts(); - // void resetEncoder(); + + /** + * @brief Sends the motor to run to a target encoder value at a given power without blocking + * + * @param targetCounts The target encoder value for the motor + * @param power The power to move the motor at (Bounded between [-1, 1]) + */ void runToTarget(unsigned int targetCounts, float power); + + /** + * @brief Sends the motor to run to a target encoder value at a given power + * + * @param targetCounts The target encoder value for the motor + * @param power The power to move the motor at (Bounded between [-1, 1]) + * @param block Specifies whether or not this action should block until it is complete + */ void runToTarget(unsigned int targetCounts, float power, bool block); + + /** + * @brief Sends the motor to run to a specified position at a given power + * + * @param rads The position to send the motor to (specified in radians) + * @param power The power to move the motor at (Bounded between [-1, 1]) + */ void runToTarget(double rads, float power); + + /** + * @brief Get the current state of the ArmMotor + * + * @return MotorState The current state of the ArmMotor + */ MotorState getMotorState(); + + /** + * @brief Set the motor power + * + * @param power The power to set the motor at (Bounded between [-1, 1]) + */ void setPower(float power); + + /** + * @brief Get the radian measure of the current motor + * + * @return float The radian measure of the current motor's position + */ float getRads(); + + /** + * @brief Get the name of the ArmMotor + * + * @return std::string The name of the ArmMotor + */ std::string getMotorName(); + + /** + * @brief Checks if the motor is currently within a pre-specified tolerance of a target + * + * @param targetCounts The target to test against + * @return true The motor was within the target tolerance + * @return false The motor was outside of the target tolerance + */ bool hasReachedTarget(unsigned int targetCounts); + + /** + * @brief Checks if the motor is currently within a given tolerance of a target + * + * @param targetCounts The target to test against + * @param tolerance The tolerance to give when testing the target + * @return true The motor was within the target tolerance + * @return false The motor was outside the target tolerance + */ bool hasReachedTarget(unsigned int targetCounts, unsigned int tolerance); float getPower(); }; \ No newline at end of file From 915fc6247cec914358e30e29267a55ff706db659 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 5 Apr 2021 19:33:54 -0500 Subject: [PATCH 47/85] Added doxygen comments to ArmMotor.cpp, removed unecessary methods and logic. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 43 +++++++++++++++++------ 1 file changed, 32 insertions(+), 11 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 70c5e2dd..b836d44e 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -6,68 +6,81 @@ */ #include "ArmMotor.hpp" +/// Allow for referencing the UInt32 message type easier #define Std_UInt32 std_msgs::UInt32::ConstPtr& +/// The current COUNTS_PER_ROTATION is UINT32_MAX unsigned int const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; +/// The current encoders are absolute, and so can only perform one rotation unsigned int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; template T ArmMotor::corrMod(T i, T j){ + // Figure out which way to move i to [0,j) int dir = -((i-j>0)-(i-j<0)); + // Move i to [0,j) with j-sized steps while(i<0||i>=j)i+=dir*j; + // Return the in-bounds i return i; } +/// Currently consistent with the rad->enc equation as specified here. unsigned int ArmMotor::radToEnc(double rads){ return ArmMotor::COUNTS_PER_ROTATION*ArmMotor::corrMod(rads,2 * M_PI)/(2 * M_PI); } +/// Currently consistent with the enc->rad equation as specified here. float ArmMotor::getRads(){ return ArmMotor::corrMod(this->getEncoderCounts() / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ - // std::cout<<"I ran "/*mtr:"<getMotorName()<<*/" val: "<data<encoderVal = msg->data; - // std::cout<<"value stored: "<encoderVal<getEncoderCounts()< 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; + // Set the current name, controller, and motor ID this->motorName = motorName; this->controllerID = controllerID; this->motorID = motorID; + // All motors start STOPped this->currState = MotorState::STOP; + // Create the topic string prefix for WRoboclaw controllers std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); + // Create the appropriate encoder-reading and speed-publishing subscribers and advertisers, respectfully this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); } -// ~ArmMotor(); - unsigned int ArmMotor::getEncoderCounts(){ return this->encoderVal; } -// void resetEncoder(); - void ArmMotor::runToTarget(unsigned int targetCounts, float power){ this->runToTarget(targetCounts, power, false); } bool ArmMotor::hasReachedTarget(unsigned int targetCounts, unsigned int tolerance){ - unsigned int lBound = ((targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1] < 0 ? 2 * targetCounts - tolerance : targetCounts - tolerance) % ArmMotor::ENCODER_BOUNDS[1]; - unsigned int uBound = (targetCounts + tolerance) % ArmMotor::ENCODER_BOUNDS[1]; + // Compute the upper and lower bounds in the finite encoder space + unsigned int lBound = ArmMotor::corrMod(targetCounts - tolerance, ArmMotor::ENCODER_BOUNDS[1]); + unsigned int uBound = ArmMotor::corrMod(targetCounts + tolerance, ArmMotor::ENCODER_BOUNDS[1]); + // If the computed lower bound is lower than the upper bound, perform the computation normally if(lBound < uBound) return this->getEncoderCounts() <= uBound && this->getEncoderCounts() >=lBound; + // Otherwise, check if the value is outside either bound and negate the response else return this->getEncoderCounts() <= uBound || this->getEncoderCounts() >=lBound; } +/// Current tolerance is ±0.1 degree w.r.t. the current number of counts per rotation bool ArmMotor::hasReachedTarget(unsigned int targetCounts){ return ArmMotor::hasReachedTarget(targetCounts, 1200000); } @@ -76,27 +89,35 @@ MotorState ArmMotor::getMotorState(){ return this->currState; } +/// This method auto-publishes the speed command to the WRoboclaws void ArmMotor::setPower(float power){ + // Check the bounds of the parameter if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; + // Set up and send the speed message this->powerMsg = new std_msgs::Int16(); this->powerMsg->data = power * INT16_MAX; this->speedPub.publish(*(this->powerMsg)); + // Update the current motor state based on the power command input this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; } void ArmMotor::runToTarget(unsigned int targetCounts, float power, bool block){ + // If we are not at our target... if(!this->hasReachedTarget(targetCounts)){ + // Set the power in the correct direction and continue running to the target long int direction = targetCounts - this->getEncoderCounts(); power = abs(power) * (corrMod(direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) ? 1 : -1); this->setPower(power); this->currState = MotorState::RUN_TO_TARGET; - } - if(this->hasReachedTarget(targetCounts)){ + // Otherwise, stop the motor + }else{ this->setPower(0.f); this->currState = MotorState::STOP; } + // If this is a blocking call... if(block){ + // Wait until the motor has reached the target, then stop while(!this->hasReachedTarget(targetCounts)); this->setPower(0.f); this->currState = MotorState::STOP; From c074968e5d46bd8fff40edb40cebe7d68878f03b Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 7 Apr 2021 18:27:36 -0500 Subject: [PATCH 48/85] Updated ArmMotor::corrMod to a simpler implementation. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index b836d44e..8f24fc1b 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -15,12 +15,8 @@ unsigned int const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; unsigned int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; template T ArmMotor::corrMod(T i, T j){ - // Figure out which way to move i to [0,j) - int dir = -((i-j>0)-(i-j<0)); - // Move i to [0,j) with j-sized steps - while(i<0||i>=j)i+=dir*j; - // Return the in-bounds i - return i; + // Stem i%j by j, which in modular arithmetic is the same as adding 0. + return fmod(fmod(i,j)+j,j); } /// Currently consistent with the rad->enc equation as specified here. From 622613f4a650b809541ddd1bec4c006131be1761 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 7 Apr 2021 20:06:55 -0500 Subject: [PATCH 49/85] Added velocity scaling to the arm, assumed speed 0.1 on the last motion. Added debugging information relavant to velocity scaling. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 11 +++++++++-- test_arm_control_stack.sh | 2 +- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 7eda7425..3a0430a5 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "ArmMotor.hpp" /** @@ -55,22 +56,28 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server names.clear(); positions.clear(); + double velMax = 0; // For each motor specified in the currTargetPosition... for(int j = 0; j < currTargetPosition.positions.size(); j++){ // Each motor should run to its respective target position at a fixed speed // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters - motors[j]->runToTarget(currTargetPosition.positions[j], 0.1); + velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)runToTarget(currTargetPosition.positions[j], currPower); // The position has only finished if every motor is STOPped hasPositionFinished &= motors[j]->getMotorState() == MotorState::STOP; // Push the current motor name and position data to the Joint State data tracking list names.push_back(motors[j]->getMotorName()); positions.push_back(motors[j]->getRads()); - // DEBUGGING OUTPUT: Print each motor's name, radian position, and encoder position + // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()</dev/null 2>&1 && pwd )" cd $ROOT_DIR -catkin_make +./assemble.py build gnome-terminal -- roscore rostopic list &>./out.temp; COUNT=$(grep -Ec "Unable to communicate with master" out.temp) From 5c59da11af538261523e2bdf3ea2c79e227202a7 Mon Sep 17 00:00:00 2001 From: Ben Nowotny <44074469+bennowotny@users.noreply.github.com> Date: Thu, 8 Apr 2021 18:51:21 -0500 Subject: [PATCH 50/85] Added TODO for future type conversion The future type conversion creates platform independence, since only 64 bit processors have 32 bit integers. (32 bit processors would throw errors) --- src/wr_control_drive_arm/src/ArmMotor.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 5ad74f23..9e0e458e 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -25,6 +25,7 @@ enum class MotorState{ /** * @brief A way to control arm motors with WRoboclaw */ +// TODO: Update the encoder datatypes to be uint32_t instead of unsigned int for platform independence. class ArmMotor{ private: /// The number of encoder counts per rotation @@ -163,4 +164,4 @@ class ArmMotor{ */ bool hasReachedTarget(unsigned int targetCounts, unsigned int tolerance); float getPower(); -}; \ No newline at end of file +}; From 81eb47ea377bfb43ab6c7e38fd233840f965c573 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 12 Apr 2021 19:06:57 -0500 Subject: [PATCH 51/85] Updated TODO by migrating encoder data types to `uint32_t` for platform independence. --- src/wr_control_drive_arm/src/ArmMotor.cpp | 20 ++++++++++---------- src/wr_control_drive_arm/src/ArmMotor.hpp | 23 +++++++++++------------ 2 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 8f24fc1b..a919cf16 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -10,9 +10,9 @@ #define Std_UInt32 std_msgs::UInt32::ConstPtr& /// The current COUNTS_PER_ROTATION is UINT32_MAX -unsigned int const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; +uint32_t const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; /// The current encoders are absolute, and so can only perform one rotation -unsigned int const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; +uint32_t const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; template T ArmMotor::corrMod(T i, T j){ // Stem i%j by j, which in modular arithmetic is the same as adding 0. @@ -20,7 +20,7 @@ template T ArmMotor::corrMod(T i, T j){ } /// Currently consistent with the rad->enc equation as specified here. -unsigned int ArmMotor::radToEnc(double rads){ +uint32_t ArmMotor::radToEnc(double rads){ return ArmMotor::COUNTS_PER_ROTATION*ArmMotor::corrMod(rads,2 * M_PI)/(2 * M_PI); } @@ -56,18 +56,18 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); } -unsigned int ArmMotor::getEncoderCounts(){ +uint32_t ArmMotor::getEncoderCounts(){ return this->encoderVal; } -void ArmMotor::runToTarget(unsigned int targetCounts, float power){ +void ArmMotor::runToTarget(uint32_t targetCounts, float power){ this->runToTarget(targetCounts, power, false); } -bool ArmMotor::hasReachedTarget(unsigned int targetCounts, unsigned int tolerance){ +bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance){ // Compute the upper and lower bounds in the finite encoder space - unsigned int lBound = ArmMotor::corrMod(targetCounts - tolerance, ArmMotor::ENCODER_BOUNDS[1]); - unsigned int uBound = ArmMotor::corrMod(targetCounts + tolerance, ArmMotor::ENCODER_BOUNDS[1]); + uint32_t lBound = ArmMotor::corrMod(targetCounts - tolerance, ArmMotor::ENCODER_BOUNDS[1]); + uint32_t uBound = ArmMotor::corrMod(targetCounts + tolerance, ArmMotor::ENCODER_BOUNDS[1]); // If the computed lower bound is lower than the upper bound, perform the computation normally if(lBound < uBound) return this->getEncoderCounts() <= uBound && this->getEncoderCounts() >=lBound; @@ -77,7 +77,7 @@ bool ArmMotor::hasReachedTarget(unsigned int targetCounts, unsigned int toleranc } /// Current tolerance is ±0.1 degree w.r.t. the current number of counts per rotation -bool ArmMotor::hasReachedTarget(unsigned int targetCounts){ +bool ArmMotor::hasReachedTarget(uint32_t targetCounts){ return ArmMotor::hasReachedTarget(targetCounts, 1200000); } @@ -98,7 +98,7 @@ void ArmMotor::setPower(float power){ this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; } -void ArmMotor::runToTarget(unsigned int targetCounts, float power, bool block){ +void ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ // If we are not at our target... if(!this->hasReachedTarget(targetCounts)){ // Set the power in the correct direction and continue running to the target diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 9e0e458e..668eaf17 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -25,13 +25,12 @@ enum class MotorState{ /** * @brief A way to control arm motors with WRoboclaw */ -// TODO: Update the encoder datatypes to be uint32_t instead of unsigned int for platform independence. class ArmMotor{ private: /// The number of encoder counts per rotation - static unsigned int const COUNTS_PER_ROTATION; + static uint32_t const COUNTS_PER_ROTATION; /// The upper and lower bounds of encoder rotation for absolute encoders (index 0 is lower, index 1 is upper) - static unsigned int const ENCODER_BOUNDS[2]; + static uint32_t const ENCODER_BOUNDS[2]; /// The current state of the motor MotorState currState; /// The joint name of the current motor @@ -41,7 +40,7 @@ class ArmMotor{ /// The ID of the motor within the WRoboclaw controller unsigned int motorID; /// The current encoder value - unsigned int encoderVal; + uint32_t encoderVal; /// The ROS Subscriber that reads the encoders ros::Subscriber encRead; /// The ROS Publisher that publishes motor speed commands @@ -53,9 +52,9 @@ class ArmMotor{ * @brief A static conversion from radians to encoder counts * * @param rad The input number of radians - * @return unsigned int The corresponding encoder count bounded by ENCODER_BOUNDS + * @return uint32_t The corresponding encoder count bounded by ENCODER_BOUNDS */ - static unsigned int radToEnc(double rad); + static uint32_t radToEnc(double rad); /** * @brief Subscriber callback for encRead, captured the encoder value of the current motor @@ -88,9 +87,9 @@ class ArmMotor{ /** * @brief Gets the encoder value of the motor * - * @return unsigned int The current encoder value of the motor + * @return uint32_t The current encoder value of the motor */ - unsigned int getEncoderCounts(); + uint32_t getEncoderCounts(); /** * @brief Sends the motor to run to a target encoder value at a given power without blocking @@ -98,7 +97,7 @@ class ArmMotor{ * @param targetCounts The target encoder value for the motor * @param power The power to move the motor at (Bounded between [-1, 1]) */ - void runToTarget(unsigned int targetCounts, float power); + void runToTarget(uint32_t targetCounts, float power); /** * @brief Sends the motor to run to a target encoder value at a given power @@ -107,7 +106,7 @@ class ArmMotor{ * @param power The power to move the motor at (Bounded between [-1, 1]) * @param block Specifies whether or not this action should block until it is complete */ - void runToTarget(unsigned int targetCounts, float power, bool block); + void runToTarget(uint32_t targetCounts, float power, bool block); /** * @brief Sends the motor to run to a specified position at a given power @@ -152,7 +151,7 @@ class ArmMotor{ * @return true The motor was within the target tolerance * @return false The motor was outside of the target tolerance */ - bool hasReachedTarget(unsigned int targetCounts); + bool hasReachedTarget(uint32_t targetCounts); /** * @brief Checks if the motor is currently within a given tolerance of a target @@ -162,6 +161,6 @@ class ArmMotor{ * @return true The motor was within the target tolerance * @return false The motor was outside the target tolerance */ - bool hasReachedTarget(unsigned int targetCounts, unsigned int tolerance); + bool hasReachedTarget(uint32_t targetCounts, uint32_t tolerance); float getPower(); }; From 26ee8aedba9fda1e93388d500a57420111ffadc7 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 15 Apr 2021 20:09:25 -0500 Subject: [PATCH 52/85] Added a signal handler to kill the action server if `^C` or `SIGINT` signal is given. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 3a0430a5..262ba94d 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "ArmMotor.hpp" /** @@ -23,6 +24,10 @@ ros::Publisher jointStatePublisher; * @brief Simplify the SimpleActionServer reference name */ typedef actionlib::SimpleActionServer Server; +/** + * @brief The ActionServer that executes the given motions + */ +Server* server; /** * @brief Perform the given action as interpreted as moving the arm joints to specified positions @@ -122,9 +127,11 @@ int main(int argc, char** argv) jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); // Initialize the Action Server - Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); + server = new Server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, server), false); // Start the Action Server - server.start(); + server->start(); + + signal(SIGINT, [](int signal)->void{ros::shutdown(); exit(1);}); // ROS spin for communication with other nodes ros::spin(); From 9a253989017da25e4078e5652f2a0964564215d2 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 20 May 2021 18:43:54 -0500 Subject: [PATCH 53/85] Adding support for PID position control to arm joints. --- .gitignore | 2 + project.json | 7 ++ .../config/arm_motor_PID.yaml | 73 +++++++++++++++++++ src/wr_control_drive_arm/launch/std.launch | 6 ++ src/wr_control_drive_arm/src/ArmMotor.cpp | 20 ++++- src/wr_control_drive_arm/src/ArmMotor.hpp | 16 +++- test_arm_control_stack.sh | 2 +- 7 files changed, 121 insertions(+), 5 deletions(-) create mode 100644 src/wr_control_drive_arm/config/arm_motor_PID.yaml create mode 100644 src/wr_control_drive_arm/launch/std.launch diff --git a/.gitignore b/.gitignore index 826e512f..d217bac1 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,7 @@ src/sick_scan src/wreadinput src/wready src/wroboclaw +src/wresponsecontrol # external lib destination dirs src/drive/lib @@ -28,3 +29,4 @@ src/science/lib *.pyc src/arm/lib +compile_commands.json diff --git a/project.json b/project.json index 09395d6f..26a6a4c6 100644 --- a/project.json +++ b/project.json @@ -20,6 +20,13 @@ "build": { "provider": "pip" } + }, + { + "name": "wresponsecontrol", + "repo": "https://github.com/WisconsinRobotics/wresponsecontrol.git", + "build": { + "provider": "noop" + } } ] } diff --git a/src/wr_control_drive_arm/config/arm_motor_PID.yaml b/src/wr_control_drive_arm/config/arm_motor_PID.yaml new file mode 100644 index 00000000..02d7f3e3 --- /dev/null +++ b/src/wr_control_drive_arm/config/arm_motor_PID.yaml @@ -0,0 +1,73 @@ +rate: 50 +controllers: + - setpointTopic: "/control/arm/00/setpoint" + feedbackTopic: "/control/arm/00/feedback" + outputTopic: "/control/arm/00/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/01/setpoint" + feedbackTopic: "/control/arm/01/feedback" + outputTopic: "/control/arm/01/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/10/setpoint" + feedbackTopic: "/control/arm/10/feedback" + outputTopic: "/control/arm/10/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/11/setpoint" + feedbackTopic: "/control/arm/11/feedback" + outputTopic: "/control/arm/11/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/20/setpoint" + feedbackTopic: "/control/arm/20/feedback" + outputTopic: "/control/arm/20/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/21/setpoint" + feedbackTopic: "/control/arm/21/feedback" + outputTopic: "/control/arm/21/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/30/setpoint" + feedbackTopic: "/control/arm/30/feedback" + outputTopic: "/control/arm/30/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 + + - setpointTopic: "/control/arm/31/setpoint" + feedbackTopic: "/control/arm/31/feedback" + outputTopic: "/control/arm/31/output" + P: 0.000001 + I: 0 + D: 0 + min: -1 + max: 1 \ No newline at end of file diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch new file mode 100644 index 00000000..a124c79c --- /dev/null +++ b/src/wr_control_drive_arm/launch/std.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index a919cf16..7e90cd80 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -8,6 +8,7 @@ /// Allow for referencing the UInt32 message type easier #define Std_UInt32 std_msgs::UInt32::ConstPtr& +#define Std_Float64 std_msgs::Float64::ConstPtr& /// The current COUNTS_PER_ROTATION is UINT32_MAX uint32_t const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; @@ -32,6 +33,15 @@ float ArmMotor::getRads(){ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ // Store the message value in this ArmMotor's encoderVal variable this->encoderVal = msg->data; + // Send feedback + std_msgs::Float64 feedbackMsg; + feedbackMsg.data = msg->data; + this->feedbackPub.publish(feedbackMsg); +} + +void ArmMotor::redirectPowerOutput(const Std_Float64 msg){ + // Set the speed to be the contained data + this->setPower(msg->data); } /// controllerID is constrained between [0,3] @@ -50,10 +60,14 @@ ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned in // Create the topic string prefix for WRoboclaw controllers std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); + std::string controlString = "/control/arm/" + std::to_string(controllerID) + std::to_string(motorID); // Create the appropriate encoder-reading and speed-publishing subscribers and advertisers, respectfully this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); + this->targetPub = n->advertise(controlString + "/setpoint", 1000); + this->feedbackPub = n->advertise(controlString + "/feedback", 1000); + this->outputRead = n->subscribe(controlString + "/output", 1000, &ArmMotor::redirectPowerOutput, this); } uint32_t ArmMotor::getEncoderCounts(){ @@ -102,9 +116,9 @@ void ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ // If we are not at our target... if(!this->hasReachedTarget(targetCounts)){ // Set the power in the correct direction and continue running to the target - long int direction = targetCounts - this->getEncoderCounts(); - power = abs(power) * (corrMod(direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)ArmMotor::COUNTS_PER_ROTATION)) ? 1 : -1); - this->setPower(power); + std_msgs::Float64 setpointMsg; + setpointMsg.data = targetCounts; + this->targetPub.publish(setpointMsg); this->currState = MotorState::RUN_TO_TARGET; // Otherwise, stop the motor }else{ diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 668eaf17..db1e51b3 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -8,6 +8,7 @@ #include "ros/ros.h" #include "std_msgs/UInt32.h" #include "std_msgs/Int16.h" +#include "std_msgs/Float64.h" #include "math.h" #include @@ -43,6 +44,12 @@ class ArmMotor{ uint32_t encoderVal; /// The ROS Subscriber that reads the encoders ros::Subscriber encRead; + /// The ROS Publisher that sets the encoder targets + ros::Publisher targetPub; + /// The ROS Publisher that sets encoder feedback data + ros::Publisher feedbackPub; + /// The ROS Subscriber that reads controlled output data + ros::Subscriber outputRead; /// The ROS Publisher that publishes motor speed commands ros::Publisher speedPub; /// A pointer to the most recent power message sent @@ -57,12 +64,19 @@ class ArmMotor{ static uint32_t radToEnc(double rad); /** - * @brief Subscriber callback for encRead, captured the encoder value of the current motor + * @brief Subscriber callback for encRead, captures the encoder value of the current motor * * @param msg The encoder value message as captured by encRead */ void storeEncoderVals(const std_msgs::UInt32::ConstPtr& msg); + /** + * @brief Subscriber callback for outputRead, captures the PID output and sets the speed directly + * + * @param msg The PID output as captured by outputRead + */ + void redirectPowerOutput(const std_msgs::Float64::ConstPtr& msg); + /** * @brief Performs Euclidean correct modulus between two inputs of the same type * diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index e3b08717..d536f673 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -12,6 +12,6 @@ done rm ./out.temp; export WROVER_LOCAL=true gnome-terminal -- roslaunch wr_entry_point test_arm.launch -gnome-terminal -- rosrun wr_control_drive_arm ArmControlSystem +gnome-terminal -- roslaunch wr_control_drive_arm std.launch gnome-terminal -- roslaunch wroboarm_21 demo_test.launch cd - From f309e7785880f117f38570c99bf1dd02deb6bbca Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 20 May 2021 20:52:03 -0500 Subject: [PATCH 54/85] Updated PID calculation method to use radians instead of encoder counts, resolved pointer-based error occurring when server.setSucceeded is called. --- .../config/arm_motor_PID.yaml | 30 ++++--------------- src/wr_control_drive_arm/launch/std.launch | 2 +- .../src/ArmControlSystem.cpp | 16 ++++------ src/wr_control_drive_arm/src/ArmMotor.cpp | 10 +++++-- src/wr_control_drive_arm/src/ArmMotor.hpp | 8 +++++ 5 files changed, 28 insertions(+), 38 deletions(-) diff --git a/src/wr_control_drive_arm/config/arm_motor_PID.yaml b/src/wr_control_drive_arm/config/arm_motor_PID.yaml index 02d7f3e3..4cd3ea1f 100644 --- a/src/wr_control_drive_arm/config/arm_motor_PID.yaml +++ b/src/wr_control_drive_arm/config/arm_motor_PID.yaml @@ -3,7 +3,7 @@ controllers: - setpointTopic: "/control/arm/00/setpoint" feedbackTopic: "/control/arm/00/feedback" outputTopic: "/control/arm/00/output" - P: 0.000001 + P: 1 I: 0 D: 0 min: -1 @@ -12,7 +12,7 @@ controllers: - setpointTopic: "/control/arm/01/setpoint" feedbackTopic: "/control/arm/01/feedback" outputTopic: "/control/arm/01/output" - P: 0.000001 + P: 1 I: 0 D: 0 min: -1 @@ -21,7 +21,7 @@ controllers: - setpointTopic: "/control/arm/10/setpoint" feedbackTopic: "/control/arm/10/feedback" outputTopic: "/control/arm/10/output" - P: 0.000001 + P: 1 I: 0 D: 0 min: -1 @@ -30,7 +30,7 @@ controllers: - setpointTopic: "/control/arm/11/setpoint" feedbackTopic: "/control/arm/11/feedback" outputTopic: "/control/arm/11/output" - P: 0.000001 + P: 1 I: 0 D: 0 min: -1 @@ -39,7 +39,7 @@ controllers: - setpointTopic: "/control/arm/20/setpoint" feedbackTopic: "/control/arm/20/feedback" outputTopic: "/control/arm/20/output" - P: 0.000001 + P: 1 I: 0 D: 0 min: -1 @@ -48,25 +48,7 @@ controllers: - setpointTopic: "/control/arm/21/setpoint" feedbackTopic: "/control/arm/21/feedback" outputTopic: "/control/arm/21/output" - P: 0.000001 - I: 0 - D: 0 - min: -1 - max: 1 - - - setpointTopic: "/control/arm/30/setpoint" - feedbackTopic: "/control/arm/30/feedback" - outputTopic: "/control/arm/30/output" - P: 0.000001 - I: 0 - D: 0 - min: -1 - max: 1 - - - setpointTopic: "/control/arm/31/setpoint" - feedbackTopic: "/control/arm/31/feedback" - outputTopic: "/control/arm/31/output" - P: 0.000001 + P: 1 I: 0 D: 0 min: -1 diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch index a124c79c..c1f32a09 100644 --- a/src/wr_control_drive_arm/launch/std.launch +++ b/src/wr_control_drive_arm/launch/std.launch @@ -2,5 +2,5 @@ - + \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 262ba94d..c1c59425 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -24,10 +24,6 @@ ros::Publisher jointStatePublisher; * @brief Simplify the SimpleActionServer reference name */ typedef actionlib::SimpleActionServer Server; -/** - * @brief The ActionServer that executes the given motions - */ -Server* server; /** * @brief Perform the given action as interpreted as moving the arm joints to specified positions @@ -66,10 +62,10 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server for(int j = 0; j < currTargetPosition.positions.size(); j++){ // Each motor should run to its respective target position at a fixed speed // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters - velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)runToTarget(currTargetPosition.positions[j], currPower); + // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)runToTarget(currTargetPosition.positions[j], 0);//currPower); // The position has only finished if every motor is STOPped hasPositionFinished &= motors[j]->getMotorState() == MotorState::STOP; // Push the current motor name and position data to the Joint State data tracking list @@ -127,9 +123,9 @@ int main(int argc, char** argv) jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); // Initialize the Action Server - server = new Server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, server), false); + Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); // Start the Action Server - server->start(); + server.start(); signal(SIGINT, [](int signal)->void{ros::shutdown(); exit(1);}); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 7e90cd80..6462f634 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -25,9 +25,13 @@ uint32_t ArmMotor::radToEnc(double rads){ return ArmMotor::COUNTS_PER_ROTATION*ArmMotor::corrMod(rads,2 * M_PI)/(2 * M_PI); } +double ArmMotor::encToRad(uint32_t enc){ + return ArmMotor::corrMod(enc / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; +} + /// Currently consistent with the enc->rad equation as specified here. float ArmMotor::getRads(){ - return ArmMotor::corrMod(this->getEncoderCounts() / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; + return ArmMotor::encToRad(this->getEncoderCounts()); } void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ @@ -35,7 +39,7 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ this->encoderVal = msg->data; // Send feedback std_msgs::Float64 feedbackMsg; - feedbackMsg.data = msg->data; + feedbackMsg.data = ArmMotor::encToRad(msg->data); this->feedbackPub.publish(feedbackMsg); } @@ -117,7 +121,7 @@ void ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ if(!this->hasReachedTarget(targetCounts)){ // Set the power in the correct direction and continue running to the target std_msgs::Float64 setpointMsg; - setpointMsg.data = targetCounts; + setpointMsg.data = ArmMotor::encToRad(targetCounts); this->targetPub.publish(setpointMsg); this->currState = MotorState::RUN_TO_TARGET; // Otherwise, stop the motor diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index db1e51b3..1819e217 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -63,6 +63,14 @@ class ArmMotor{ */ static uint32_t radToEnc(double rad); + /** + * @brief A static conversion from encoder counts to radians + * + * @param enc The input number of encoder counts + * @return double The corresponding radian measure + */ + static double encToRad(uint32_t enc); + /** * @brief Subscriber callback for encRead, captures the encoder value of the current motor * From fbd4279512b8bd2b2d87df5b60be1178f9af478e Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 24 May 2021 18:43:12 -0500 Subject: [PATCH 55/85] Updating the arm URDF files to contain joint limits to prevent overturning. --- src/wroboarm_21/config/cartesian_limits.yaml | 5 + ...otion_planner_planning_pipeline.launch.xml | 19 + src/wroboarm_21/urdf/wroboarm_21.urdf | 641 ++++++++---------- 3 files changed, 303 insertions(+), 362 deletions(-) create mode 100644 src/wroboarm_21/config/cartesian_limits.yaml create mode 100644 src/wroboarm_21/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml diff --git a/src/wroboarm_21/config/cartesian_limits.yaml b/src/wroboarm_21/config/cartesian_limits.yaml new file mode 100644 index 00000000..7df72f69 --- /dev/null +++ b/src/wroboarm_21/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/src/wroboarm_21/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/src/wroboarm_21/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 00000000..9478782e --- /dev/null +++ b/src/wroboarm_21/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf index e15f105b..27bf0988 100644 --- a/src/wroboarm_21/urdf/wroboarm_21.urdf +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -1,365 +1,282 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + / + + + From 0502500b8f2b75db911a76b7a423f2541a632005 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 24 May 2021 18:44:10 -0500 Subject: [PATCH 56/85] Adding rqt_plot to the arm launch script to see joint state data and controller responses. --- test_arm_control_stack.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index d536f673..b2b3a6a9 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -14,4 +14,5 @@ export WROVER_LOCAL=true gnome-terminal -- roslaunch wr_entry_point test_arm.launch gnome-terminal -- roslaunch wr_control_drive_arm std.launch gnome-terminal -- roslaunch wroboarm_21 demo_test.launch +gnome-terminal -- rqt_plot /control/arm/00/setpoint /control/arm/00/feedback cd - From 98aee31012120468dc62f5f94eccda5432397a7d Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 24 May 2021 18:48:55 -0500 Subject: [PATCH 57/85] Adding support for PID control, post-testing. --- src/wr_control_drive_arm/launch/std.launch | 2 +- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch index c1f32a09..a124c79c 100644 --- a/src/wr_control_drive_arm/launch/std.launch +++ b/src/wr_control_drive_arm/launch/std.launch @@ -2,5 +2,5 @@ - + \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index c1c59425..4ef19760 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -127,7 +127,7 @@ int main(int argc, char** argv) // Start the Action Server server.start(); - signal(SIGINT, [](int signal)->void{ros::shutdown(); exit(1);}); + // signal(SIGINT, [](int signal)->void{ros::shutdown(); exit(1);}); // ROS spin for communication with other nodes ros::spin(); From ff5274ad4f888bdcfdea819c2abc7531dec8e126 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 24 May 2021 18:49:57 -0500 Subject: [PATCH 58/85] PID Tuning. --- src/wr_control_drive_arm/config/arm_motor_PID.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/wr_control_drive_arm/config/arm_motor_PID.yaml b/src/wr_control_drive_arm/config/arm_motor_PID.yaml index 4cd3ea1f..bf359534 100644 --- a/src/wr_control_drive_arm/config/arm_motor_PID.yaml +++ b/src/wr_control_drive_arm/config/arm_motor_PID.yaml @@ -3,7 +3,7 @@ controllers: - setpointTopic: "/control/arm/00/setpoint" feedbackTopic: "/control/arm/00/feedback" outputTopic: "/control/arm/00/output" - P: 1 + P: 30 I: 0 D: 0 min: -1 @@ -12,7 +12,7 @@ controllers: - setpointTopic: "/control/arm/01/setpoint" feedbackTopic: "/control/arm/01/feedback" outputTopic: "/control/arm/01/output" - P: 1 + P: 30 I: 0 D: 0 min: -1 @@ -21,7 +21,7 @@ controllers: - setpointTopic: "/control/arm/10/setpoint" feedbackTopic: "/control/arm/10/feedback" outputTopic: "/control/arm/10/output" - P: 1 + P: 30 I: 0 D: 0 min: -1 @@ -30,7 +30,7 @@ controllers: - setpointTopic: "/control/arm/11/setpoint" feedbackTopic: "/control/arm/11/feedback" outputTopic: "/control/arm/11/output" - P: 1 + P: 30 I: 0 D: 0 min: -1 @@ -39,7 +39,7 @@ controllers: - setpointTopic: "/control/arm/20/setpoint" feedbackTopic: "/control/arm/20/feedback" outputTopic: "/control/arm/20/output" - P: 1 + P: 30 I: 0 D: 0 min: -1 @@ -48,7 +48,7 @@ controllers: - setpointTopic: "/control/arm/21/setpoint" feedbackTopic: "/control/arm/21/feedback" outputTopic: "/control/arm/21/output" - P: 1 + P: 30 I: 0 D: 0 min: -1 From a1070ba54a4886e6aa6221cd74fa00fad3b264da Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 24 May 2021 20:16:46 -0500 Subject: [PATCH 59/85] Updated arm teleop logic, currently segfaults. --- src/wr_logic_teleop_arm/CMakeLists.txt | 4 ++++ src/wr_logic_teleop_arm/package.xml | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/wr_logic_teleop_arm/CMakeLists.txt b/src/wr_logic_teleop_arm/CMakeLists.txt index c061b7c5..1ec82fc7 100644 --- a/src/wr_logic_teleop_arm/CMakeLists.txt +++ b/src/wr_logic_teleop_arm/CMakeLists.txt @@ -10,6 +10,8 @@ project(wr_logic_teleop_arm) find_package(catkin REQUIRED COMPONENTS roscpp rospy + moveit_msgs + moveit_visual_tools ) ## System dependencies are found with CMake's conventions @@ -133,6 +135,8 @@ include_directories( ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/wr_logic_teleop_arm_node.cpp) +add_executable(ArmTeleopLogic src/ArmTeleopLogic.cpp) +target_link_libraries(ArmTeleopLogic ${catkin_LIBRARIES}) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_logic_teleop_arm/package.xml b/src/wr_logic_teleop_arm/package.xml index c2fc4d95..6c5b0cad 100644 --- a/src/wr_logic_teleop_arm/package.xml +++ b/src/wr_logic_teleop_arm/package.xml @@ -55,6 +55,8 @@ rospy roscpp rospy + moveit_msgs + moveit_visual_tools From 63974607930a33ed2c18497a6c8a1f909ac2935a Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 24 May 2021 20:17:33 -0500 Subject: [PATCH 60/85] Added the arm teleop file (the one that currently segfaults). --- .../src/ArmTeleopLogic.cpp | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp new file mode 100644 index 00000000..c5a9df54 --- /dev/null +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "ArmTeleopLogic"); + ros::AsyncSpinner spin(1); + spin.start(); + ros::NodeHandle np("~"); + + robot_model_loader::RobotModelLoader modelLoader("robot_description"); + const robot_model::RobotModelPtr model = modelLoader.getModel(); + moveit::core::RobotStatePtr armState(new moveit::core::RobotState(model)); + const moveit::core::JointModelGroup* joint_model_group = armState->getJointModelGroup("arm"); + + planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(model)); + scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); + + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + geometry_msgs::PoseStamped pose; + pose.header.frame_id = "link1"; + pose.pose.position.x = 3; + pose.pose.position.y = 2; + pose.pose.position.z = 1; + pose.pose.orientation.w = 0; + moveit_msgs::Constraints poseGoal = kinematic_constraints::constructGoalConstraints("link6", pose); + req.goal_constraints.clear(); + req.goal_constraints.push_back(poseGoal); + + planning_interface::PlannerManagerPtr planner; + + planning_interface::PlanningContextPtr context = planner->getPlanningContext(scene, req, res.error_code_); + context->solve(res); + if(res.error_code_.val != res.error_code_.SUCCESS){ + ROS_ERROR("Could not compute plan successfully"); + return 0; + + } + + ros::Publisher display_publisher = np.advertise("/move_group/display_planned_path", 1, true); + moveit_msgs::DisplayTrajectory display_trajectory; + moveit_msgs::MotionPlanResponse response; + res.getMessage(response); + display_trajectory.trajectory_start = response.trajectory_start; + display_trajectory.trajectory.push_back(response.trajectory); + display_publisher.publish(display_trajectory); + + moveit_visual_tools::MoveItVisualTools vTools("link1"); + vTools.deleteAllMarkers(); + vTools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); + vTools.trigger(); + + + return 0; +} \ No newline at end of file From eefef3ed729627b4e2cf972e0c1badcfcb4c3fbd Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 26 May 2021 20:29:01 -0500 Subject: [PATCH 61/85] Added a 'control' group tag to the wr_control_drive_arm nodes for organization. --- src/wr_control_drive_arm/launch/std.launch | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch index a124c79c..fe7dcb12 100644 --- a/src/wr_control_drive_arm/launch/std.launch +++ b/src/wr_control_drive_arm/launch/std.launch @@ -1,6 +1,8 @@ - - - - + + + + + + \ No newline at end of file From e650085f4dff81da5ef615b25dcc5c9b6b362481 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 26 May 2021 20:30:17 -0500 Subject: [PATCH 62/85] Added effective `rosrun` control to wr_logic_teleop_arm. --- test_arm_control_stack.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index b2b3a6a9..7a7322b3 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -15,4 +15,6 @@ gnome-terminal -- roslaunch wr_entry_point test_arm.launch gnome-terminal -- roslaunch wr_control_drive_arm std.launch gnome-terminal -- roslaunch wroboarm_21 demo_test.launch gnome-terminal -- rqt_plot /control/arm/00/setpoint /control/arm/00/feedback +sleep 5 +gnome-terminal -- /bin/sh -c "rosrun wr_logic_teleop_arm ArmTeleopLogic; while true; do sleep 1; done" cd - From db8f29aca1caa63cdba35100ce2f83c2d65131d8 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 26 May 2021 20:31:14 -0500 Subject: [PATCH 63/85] Modified the ArmTeleopLogic system to plan to a predetermined spot and move there. Confirmed to work. --- src/wr_logic_teleop_arm/CMakeLists.txt | 5 +- src/wr_logic_teleop_arm/package.xml | 2 + .../src/ArmTeleopLogic.cpp | 56 ++----------------- 3 files changed, 12 insertions(+), 51 deletions(-) diff --git a/src/wr_logic_teleop_arm/CMakeLists.txt b/src/wr_logic_teleop_arm/CMakeLists.txt index 1ec82fc7..746f428e 100644 --- a/src/wr_logic_teleop_arm/CMakeLists.txt +++ b/src/wr_logic_teleop_arm/CMakeLists.txt @@ -10,6 +10,8 @@ project(wr_logic_teleop_arm) find_package(catkin REQUIRED COMPONENTS roscpp rospy + moveit_core + moveit_ros_planning_interface moveit_msgs moveit_visual_tools ) @@ -119,6 +121,7 @@ catkin_package( include_directories( # include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} ) ## Declare a C++ library @@ -136,7 +139,7 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/wr_logic_teleop_arm_node.cpp) add_executable(ArmTeleopLogic src/ArmTeleopLogic.cpp) -target_link_libraries(ArmTeleopLogic ${catkin_LIBRARIES}) +target_link_libraries(ArmTeleopLogic ${catkin_LIBRARIES} ${Boost_LIBRARIES}) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_logic_teleop_arm/package.xml b/src/wr_logic_teleop_arm/package.xml index 6c5b0cad..d8351162 100644 --- a/src/wr_logic_teleop_arm/package.xml +++ b/src/wr_logic_teleop_arm/package.xml @@ -55,6 +55,8 @@ rospy roscpp rospy + moveit_core + moveit_ros_planning_interface moveit_msgs moveit_visual_tools diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index c5a9df54..ce4b4eb3 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -1,11 +1,5 @@ #include -#include -#include -#include -#include -#include -#include -#include +#include int main(int argc, char** argv){ ros::init(argc, argv, "ArmTeleopLogic"); @@ -13,49 +7,11 @@ int main(int argc, char** argv){ spin.start(); ros::NodeHandle np("~"); - robot_model_loader::RobotModelLoader modelLoader("robot_description"); - const robot_model::RobotModelPtr model = modelLoader.getModel(); - moveit::core::RobotStatePtr armState(new moveit::core::RobotState(model)); - const moveit::core::JointModelGroup* joint_model_group = armState->getJointModelGroup("arm"); - - planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(model)); - scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); - - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - geometry_msgs::PoseStamped pose; - pose.header.frame_id = "link1"; - pose.pose.position.x = 3; - pose.pose.position.y = 2; - pose.pose.position.z = 1; - pose.pose.orientation.w = 0; - moveit_msgs::Constraints poseGoal = kinematic_constraints::constructGoalConstraints("link6", pose); - req.goal_constraints.clear(); - req.goal_constraints.push_back(poseGoal); - - planning_interface::PlannerManagerPtr planner; - - planning_interface::PlanningContextPtr context = planner->getPlanningContext(scene, req, res.error_code_); - context->solve(res); - if(res.error_code_.val != res.error_code_.SUCCESS){ - ROS_ERROR("Could not compute plan successfully"); - return 0; - - } - - ros::Publisher display_publisher = np.advertise("/move_group/display_planned_path", 1, true); - moveit_msgs::DisplayTrajectory display_trajectory; - moveit_msgs::MotionPlanResponse response; - res.getMessage(response); - display_trajectory.trajectory_start = response.trajectory_start; - display_trajectory.trajectory.push_back(response.trajectory); - display_publisher.publish(display_trajectory); - - moveit_visual_tools::MoveItVisualTools vTools("link1"); - vTools.deleteAllMarkers(); - vTools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); - vTools.trigger(); - + moveit::planning_interface::MoveGroupInterface move("arm"); + // move.setPlannerId("RRTStar"); + move.setPlanningTime(0.5); + move.setPositionTarget(0.35, 0.35, 0.35); + move.move(); return 0; } \ No newline at end of file From 5e4574f10ede089b9bae898372564c0905463d88 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 26 May 2021 20:31:41 -0500 Subject: [PATCH 64/85] Removed the visual to set the goal of the arm from the RViz layout for cleanliness. --- src/wroboarm_21/launch/moveit.rviz | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index e859664b..4f01fd84 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -5,6 +5,7 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 + - /MotionPlanning1/Planning Request1 Splitter Ratio: 0.7425600290298462 Tree Height: 241 - Class: rviz/Help @@ -47,7 +48,6 @@ Visualization Manager: MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 0.5 MoveIt_Use_Cartesian_Path: false @@ -131,7 +131,7 @@ Visualization Manager: Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 Planning Group: arm - Query Goal State: true + Query Goal State: false Query Start State: false Show Workspace: false Start State Alpha: 1 @@ -241,7 +241,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a20000031cfc0200000007fb000000100044006900730070006c006100790073010000003d00000182000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c5000001940000018900ffffff000004900000031c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000002a20000031cfc0200000007fb000000100044006900730070006c006100790073010000003d00000182000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c5000001940000017d00ffffff000004900000031c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1848 From 90ad742baa89cbdf38e4f65e8df0045ae55b5de3 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 27 May 2021 20:30:19 -0500 Subject: [PATCH 65/85] Modified stepping logic and how the test simulation works. --- .../src/ArmTeleopLogic.cpp | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index ce4b4eb3..f987920a 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -1,4 +1,5 @@ #include +#include #include int main(int argc, char** argv){ @@ -9,9 +10,24 @@ int main(int argc, char** argv){ moveit::planning_interface::MoveGroupInterface move("arm"); // move.setPlannerId("RRTStar"); - move.setPlanningTime(0.5); - move.setPositionTarget(0.35, 0.35, 0.35); - move.move(); + move.setPlanningTime(0.15); + double x = -0.5; + int i = 0; + while(x<0.5){ + geometry_msgs::PoseStamped p; + p.pose.position.x = 0.25; + p.pose.position.y = x; + p.pose.position.z = 0.25; + p.pose.orientation.x = 0; + p.pose.orientation.y = sin(-M_PI/4); + p.pose.orientation.z = 0; + p.pose.orientation.w = cos(-M_PI/4); + p.header.frame_id = "odom_combined"; + move.setPoseTarget(p); + move.move(); + x+=0.05; + i++; + } return 0; } \ No newline at end of file From 9523473b69c2763bc724cac896f27f1b281a24e4 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 27 May 2021 20:31:40 -0500 Subject: [PATCH 66/85] Disabled `rqt_plot` tracking by comment, lengthened the timing to start the ArmTeleopLogic `rosrun`. --- test_arm_control_stack.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index 7a7322b3..c7e4795b 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -14,7 +14,7 @@ export WROVER_LOCAL=true gnome-terminal -- roslaunch wr_entry_point test_arm.launch gnome-terminal -- roslaunch wr_control_drive_arm std.launch gnome-terminal -- roslaunch wroboarm_21 demo_test.launch -gnome-terminal -- rqt_plot /control/arm/00/setpoint /control/arm/00/feedback -sleep 5 +#gnome-terminal -- rqt_plot /control/arm/00/setpoint /control/arm/00/feedback +sleep 10 gnome-terminal -- /bin/sh -c "rosrun wr_logic_teleop_arm ArmTeleopLogic; while true; do sleep 1; done" cd - From d2bd61d6deba4ea28d233a4c5ce841313f075561 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 27 May 2021 20:32:18 -0500 Subject: [PATCH 67/85] Adding the `apt` dependency to `ros-noetic-moveit` to the build script. --- assemble.py | 2 +- project.json | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/assemble.py b/assemble.py index 3442d726..f5f8c08d 100755 --- a/assemble.py +++ b/assemble.py @@ -165,7 +165,7 @@ def __init__(self, pkg_name): self.pkg_name = pkg_name def execute(self, root_dir: str, dep_dir: str, args: List[str]): - if not BuildApt.install_list: + if not hasattr(BuildApt,'install_list'): print_subsec('Caching dpkg install list...') BuildApt.install_list = set() with open('/var/lib/dpkg/status') as dpkg_status: diff --git a/project.json b/project.json index 26a6a4c6..0bf78808 100644 --- a/project.json +++ b/project.json @@ -27,6 +27,13 @@ "build": { "provider": "noop" } + }, + { + "name": "moveit", + "build": { + "package": "ros-noetic-moveit", + "provider": "apt" + } } ] } From 0ff1109990ce038db3e81165a0caea6db6251e6b Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 15 Jul 2021 21:17:52 -0500 Subject: [PATCH 68/85] Added controller functionality for the Arm (yay!). Tested with XBox-like controller. --- .../src/ArmTeleopLogic.cpp | 63 +++++++++++++++---- 1 file changed, 50 insertions(+), 13 deletions(-) diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index f987920a..3e276085 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -1,23 +1,61 @@ +#include "ros/rate.h" +#include "ros/subscriber.h" +#include "std_msgs/Bool.h" +#include "std_msgs/Float32.h" +#include "boost/function.hpp" +#include #include -#include #include -int main(int argc, char** argv){ +using Std_Bool = const std_msgs::BoolConstPtr&; +using Std_Float32 = const std_msgs::Float32ConstPtr&; + +auto main(int argc, char** argv) -> int{ ros::init(argc, argv, "ArmTeleopLogic"); ros::AsyncSpinner spin(1); spin.start(); - ros::NodeHandle np("~"); + ros::NodeHandle np("~"); + + constexpr float PLANNING_TIME = 0.05; + constexpr float CLOCK_RATE = 50; + constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000; + constexpr float TRIGGER_PRESSED = 0.5; + + constexpr float STEP_X = 0.001; + constexpr float STEP_Y = 0.001; + constexpr float STEP_Z = 0.001; + + constexpr float HOME_X = 0.25; + constexpr float HOME_Y = 0; + constexpr float HOME_Z = 0.25; + + float x_pos = HOME_X; + float y_pos = HOME_Y; + float z_pos = HOME_Z; moveit::planning_interface::MoveGroupInterface move("arm"); // move.setPlannerId("RRTStar"); - move.setPlanningTime(0.15); - double x = -0.5; - int i = 0; - while(x<0.5){ - geometry_msgs::PoseStamped p; - p.pose.position.x = 0.25; - p.pose.position.y = x; - p.pose.position.z = 0.25; + move.setPlanningTime(PLANNING_TIME); + ros::Rate loop {CLOCK_RATE}; + + ros::Subscriber yAxis = np.subscribe("/xbox_test/axis/pov_y", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void {y_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_Y : -STEP_Y : 0;})); + ros::Subscriber xAxis = np.subscribe("/xbox_test/axis/pov_x", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void {x_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_X : -STEP_X : 0;})); + ros::Subscriber zUp = np.subscribe("/xbox_test/button/shoulder_l", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void {z_pos += msg->data ? STEP_Z : 0;})); + ros::Subscriber zDown = np.subscribe("/xbox_test/axis/trigger_l", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void {z_pos += msg->data > TRIGGER_PRESSED ? -STEP_Z : 0;})); + + while(ros::ok()){ + geometry_msgs::PoseStamped p {}; + p.pose.position.x = x_pos; + p.pose.position.y = y_pos; + p.pose.position.z = z_pos; p.pose.orientation.x = 0; p.pose.orientation.y = sin(-M_PI/4); p.pose.orientation.z = 0; @@ -25,8 +63,7 @@ int main(int argc, char** argv){ p.header.frame_id = "odom_combined"; move.setPoseTarget(p); move.move(); - x+=0.05; - i++; + loop.sleep(); } return 0; From 3689fbd7bc00246182e7858d19a83549c80ec51c Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 20 Jul 2021 06:57:53 -0500 Subject: [PATCH 69/85] Adding rotational logic and controller support. --- src/wr_control_drive_science/CMakeLists.txt | 17 +++ .../src/ArmTeleopLogic.cpp | 111 ++++++++++++++++-- src/wroboarm_21/launch/moveit.rviz | 33 ++++-- test_arm_control_stack.sh | 1 + 4 files changed, 142 insertions(+), 20 deletions(-) diff --git a/src/wr_control_drive_science/CMakeLists.txt b/src/wr_control_drive_science/CMakeLists.txt index 68304cbe..d239e5b5 100644 --- a/src/wr_control_drive_science/CMakeLists.txt +++ b/src/wr_control_drive_science/CMakeLists.txt @@ -12,6 +12,10 @@ find_package(catkin REQUIRED COMPONENTS rospy ) + +find_package(Boost REQUIRED COMPONENTS python) +find_package(PythonLibs 2.7 REQUIRED) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -117,6 +121,8 @@ catkin_package( include_directories( # include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} ) ## Declare a C++ library @@ -124,6 +130,17 @@ include_directories( # src/${PROJECT_NAME}/wr_control_drive_science.cpp # ) +add_library(testModule + main.cpp +) + +target_link_libraries(testModule ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +set_target_properties(testModule PROPERTIES + PREFIX "" + LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} + ) + ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index 3e276085..a434299d 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -1,8 +1,15 @@ +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/Quaternion.h" +#include "moveit_msgs/RobotTrajectory.h" #include "ros/rate.h" #include "ros/subscriber.h" #include "std_msgs/Bool.h" #include "std_msgs/Float32.h" #include "boost/function.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Vector3.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include #include #include #include @@ -10,6 +17,21 @@ using Std_Bool = const std_msgs::BoolConstPtr&; using Std_Float32 = const std_msgs::Float32ConstPtr&; +std::atomic_bool actionClean {true}; +const tf2::Quaternion WORLD_OFFSET {0, sin(M_PI/4), 0, cos(M_PI/4)}; + +auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void{ + geometry_msgs::PoseStamped p {}; + p.pose.position.x = x_pos; + p.pose.position.y = y_pos; + p.pose.position.z = z_pos; + auto outOrientation = /* tf2::Quaternion(0,sin(M_PI/4), 0, cos(M_PI/4)) * */ orientation; + p.pose.orientation = tf2::toMsg(outOrientation); + p.header.frame_id = "turntable"; + pub.publish(p); + actionClean.store(false); +} + auto main(int argc, char** argv) -> int{ ros::init(argc, argv, "ArmTeleopLogic"); ros::AsyncSpinner spin(1); @@ -17,7 +39,7 @@ auto main(int argc, char** argv) -> int{ ros::NodeHandle np("~"); constexpr float PLANNING_TIME = 0.05; - constexpr float CLOCK_RATE = 50; + constexpr float CLOCK_RATE = 2; constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000; constexpr float TRIGGER_PRESSED = 0.5; @@ -33,37 +55,102 @@ auto main(int argc, char** argv) -> int{ float y_pos = HOME_Y; float z_pos = HOME_Z; + tf2::Quaternion orientation {0, sin(-M_PI/4), 0, cos(-M_PI/4)}; + orientation = WORLD_OFFSET * orientation; + + const tf2::Quaternion SPIN_X {sin(2*M_PI/1000), 0, 0, cos(2*M_PI/1000)}; + const tf2::Quaternion SPIN_Y {0, sin(2*M_PI/1000), 0, cos(2*M_PI/1000)}; + const tf2::Quaternion SPIN_Z {0, 0, sin(2*M_PI/1000), cos(2*M_PI/1000)}; + moveit::planning_interface::MoveGroupInterface move("arm"); // move.setPlannerId("RRTStar"); move.setPlanningTime(PLANNING_TIME); ros::Rate loop {CLOCK_RATE}; + ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", + MESSAGE_QUEUE_LENGTH); + + ros::Subscriber yAxis = np.subscribe("/xbox_test/axis/pov_y", MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void {y_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_Y : -STEP_Y : 0;})); + static_cast>([&](Std_Float32 msg) -> void { + if(msg->data != 0){ + y_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_Y : -STEP_Y : 0; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); ros::Subscriber xAxis = np.subscribe("/xbox_test/axis/pov_x", MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void {x_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_X : -STEP_X : 0;})); + static_cast>([&](Std_Float32 msg) -> void { + if(msg->data != 0){ + x_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_X : -STEP_X : 0; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); ros::Subscriber zUp = np.subscribe("/xbox_test/button/shoulder_l", MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void {z_pos += msg->data ? STEP_Z : 0;})); - ros::Subscriber zDown = np.subscribe("/xbox_test/axis/trigger_l", + static_cast>([&](Std_Bool msg) -> void { + if(msg->data){ + z_pos += msg->data ? STEP_Z : 0; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); + ros::Subscriber zDown = np.subscribe("/xbox_test/axis/trigger_left", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if(msg->data >= TRIGGER_PRESSED){ + z_pos += msg->data >= TRIGGER_PRESSED ? -STEP_Z : 0; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); + + ros::Subscriber roll = np.subscribe("/xbox_test/axis/stick_left_x", MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void {z_pos += msg->data > TRIGGER_PRESSED ? -STEP_Z : 0;})); + static_cast>([&](Std_Float32 msg) -> void { + if(abs(msg->data) >= 0.5){ + orientation *= WORLD_OFFSET.inverse() * (msg->data > 0 ? SPIN_X : SPIN_X.inverse()) * WORLD_OFFSET; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); + + ros::Subscriber pitch = np.subscribe("/xbox_test/axis/stick_left_y", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if(abs(msg->data) >= 0.5){ + orientation *= WORLD_OFFSET.inverse() * (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse()) * WORLD_OFFSET; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); + + ros::Subscriber yaw = np.subscribe("/xbox_test/axis/stick_right_x", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if(abs(msg->data) >= 0.5){ + orientation *= WORLD_OFFSET.inverse() * (msg->data > 0 ? SPIN_Z : SPIN_Z.inverse()) * WORLD_OFFSET; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + } + })); + + // transform = move.getCurrentState()->getFrameTransform("odom_combined"); + // updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); while(ros::ok()){ geometry_msgs::PoseStamped p {}; p.pose.position.x = x_pos; p.pose.position.y = y_pos; p.pose.position.z = z_pos; - p.pose.orientation.x = 0; - p.pose.orientation.y = sin(-M_PI/4); - p.pose.orientation.z = 0; - p.pose.orientation.w = cos(-M_PI/4); + p.pose.orientation = tf2::toMsg(WORLD_OFFSET.inverse() * orientation); p.header.frame_id = "odom_combined"; move.setPoseTarget(p); - move.move(); - loop.sleep(); + std::vector waypoints {p.pose}; + moveit_msgs::RobotTrajectory traj; + double discard = move.computeCartesianPath(waypoints, 0.01, 0.0, traj); + move.asyncExecute(traj); + while(!move.getMoveGroupClient().getState().isDone() && actionClean.load()) loop.sleep(); + if(!actionClean.load()) { + move.stop(); + actionClean.store(true); + } } return 0; diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index 4f01fd84..5d5f7e47 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -4,8 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /MotionPlanning1 - - /MotionPlanning1/Planning Request1 + - /Pose1 Splitter Ratio: 0.7425600290298462 Tree Height: 241 - Class: rviz/Help @@ -112,7 +113,7 @@ Visualization Manager: Robot Alpha: 0.5 Robot Color: 150; 50; 150 Show Robot Collision: false - Show Robot Visual: true + Show Robot Visual: false Show Trail: false State Display Time: 0.05 s Trail Step Size: 1 @@ -193,6 +194,22 @@ Visualization Manager: Show Robot Visual: true Value: true Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.05000000074505806 + Name: Pose + Queue Size: 10 + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.029999999329447746 + Shape: Axes + Topic: /logic/arm_teleop/next_target + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -209,7 +226,7 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.996500015258789 + Distance: 1.7158159017562866 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -217,17 +234,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.11356700211763382 - Y: 0.10592000186443329 - Z: 2.2351800055275817e-07 + X: 0.05638057738542557 + Y: 0.06656442582607269 + Z: 1.6391335577736754e-07 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4297969341278076 + Pitch: 0.4497969150543213 Target Frame: turntable - Yaw: 5.694949150085449 + Yaw: 5.75994873046875 Saved: ~ Window Geometry: Displays: diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index c7e4795b..422617d2 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -17,4 +17,5 @@ gnome-terminal -- roslaunch wroboarm_21 demo_test.launch #gnome-terminal -- rqt_plot /control/arm/00/setpoint /control/arm/00/feedback sleep 10 gnome-terminal -- /bin/sh -c "rosrun wr_logic_teleop_arm ArmTeleopLogic; while true; do sleep 1; done" +gnome-terminal -- roslaunch wreadinput test_xbox360.launch cd - From b6079260f36642e21e32fbe9be567e24384f8681 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 2 Aug 2021 19:32:00 -0500 Subject: [PATCH 70/85] Updating rotation calculations to no transformations. --- src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index a434299d..b9ae12c2 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -18,7 +18,7 @@ using Std_Bool = const std_msgs::BoolConstPtr&; using Std_Float32 = const std_msgs::Float32ConstPtr&; std::atomic_bool actionClean {true}; -const tf2::Quaternion WORLD_OFFSET {0, sin(M_PI/4), 0, cos(M_PI/4)}; +const tf2::Quaternion WORLD_OFFSET {0, sin(M_PI/2), 0, cos(M_PI/2)}; auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void{ geometry_msgs::PoseStamped p {}; @@ -56,7 +56,7 @@ auto main(int argc, char** argv) -> int{ float z_pos = HOME_Z; tf2::Quaternion orientation {0, sin(-M_PI/4), 0, cos(-M_PI/4)}; - orientation = WORLD_OFFSET * orientation; + orientation = orientation; const tf2::Quaternion SPIN_X {sin(2*M_PI/1000), 0, 0, cos(2*M_PI/1000)}; const tf2::Quaternion SPIN_Y {0, sin(2*M_PI/1000), 0, cos(2*M_PI/1000)}; @@ -108,7 +108,7 @@ auto main(int argc, char** argv) -> int{ MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { if(abs(msg->data) >= 0.5){ - orientation *= WORLD_OFFSET.inverse() * (msg->data > 0 ? SPIN_X : SPIN_X.inverse()) * WORLD_OFFSET; + orientation *= (msg->data > 0 ? SPIN_Z : SPIN_Z.inverse()); updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); } })); @@ -117,7 +117,7 @@ auto main(int argc, char** argv) -> int{ MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { if(abs(msg->data) >= 0.5){ - orientation *= WORLD_OFFSET.inverse() * (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse()) * WORLD_OFFSET; + orientation *= (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse()); updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); } })); @@ -126,7 +126,7 @@ auto main(int argc, char** argv) -> int{ MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { if(abs(msg->data) >= 0.5){ - orientation *= WORLD_OFFSET.inverse() * (msg->data > 0 ? SPIN_Z : SPIN_Z.inverse()) * WORLD_OFFSET; + orientation *= (msg->data > 0 ? SPIN_X : SPIN_X.inverse()); updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); } })); @@ -139,7 +139,7 @@ auto main(int argc, char** argv) -> int{ p.pose.position.x = x_pos; p.pose.position.y = y_pos; p.pose.position.z = z_pos; - p.pose.orientation = tf2::toMsg(WORLD_OFFSET.inverse() * orientation); + p.pose.orientation = tf2::toMsg(orientation); p.header.frame_id = "odom_combined"; move.setPoseTarget(p); std::vector waypoints {p.pose}; From 114b04c7d56007bdd9bdf8c464f34723531832af Mon Sep 17 00:00:00 2001 From: Nicholas Date: Wed, 22 Sep 2021 19:53:34 -0500 Subject: [PATCH 71/85] fixed drive science cmake files --- src/wr_control_drive_science/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/wr_control_drive_science/CMakeLists.txt b/src/wr_control_drive_science/CMakeLists.txt index d239e5b5..485cb87d 100644 --- a/src/wr_control_drive_science/CMakeLists.txt +++ b/src/wr_control_drive_science/CMakeLists.txt @@ -130,16 +130,16 @@ include_directories( # src/${PROJECT_NAME}/wr_control_drive_science.cpp # ) -add_library(testModule - main.cpp -) +# add_library(testModule +# main.cpp +# ) -target_link_libraries(testModule ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# target_link_libraries(testModule ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -set_target_properties(testModule PROPERTIES - PREFIX "" - LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} - ) +# set_target_properties(testModule PROPERTIES +# PREFIX "" +# LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +# ) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries From 33aee03e12d0b114107510dd08fc834d46ac9a95 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Fri, 24 Sep 2021 15:02:51 -0500 Subject: [PATCH 72/85] fixed teleop bug, changed vel/accel scaling, rviz panels hidden by default --- .../src/ArmTeleopLogic.cpp | 32 +++++++++++++++---- src/wroboarm_21/launch/moveit.rviz | 26 +++++++-------- 2 files changed, 38 insertions(+), 20 deletions(-) diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index b9ae12c2..d3e7c88a 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -17,7 +17,7 @@ using Std_Bool = const std_msgs::BoolConstPtr&; using Std_Float32 = const std_msgs::Float32ConstPtr&; -std::atomic_bool actionClean {true}; +std::atomic_bool isNewPath {false}; const tf2::Quaternion WORLD_OFFSET {0, sin(M_PI/2), 0, cos(M_PI/2)}; auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void{ @@ -29,7 +29,7 @@ auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orienta p.pose.orientation = tf2::toMsg(outOrientation); p.header.frame_id = "turntable"; pub.publish(p); - actionClean.store(false); + isNewPath.store(true); } auto main(int argc, char** argv) -> int{ @@ -135,6 +135,12 @@ auto main(int argc, char** argv) -> int{ // updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); while(ros::ok()){ + if(!isNewPath.load()){ + continue; + } + + // configure new trajectory + ROS_DEBUG("Hello %s", "World"); geometry_msgs::PoseStamped p {}; p.pose.position.x = x_pos; p.pose.position.y = y_pos; @@ -144,12 +150,24 @@ auto main(int argc, char** argv) -> int{ move.setPoseTarget(p); std::vector waypoints {p.pose}; moveit_msgs::RobotTrajectory traj; - double discard = move.computeCartesianPath(waypoints, 0.01, 0.0, traj); + + double discard = move.computeCartesianPath(waypoints, 0.005, 0.0, traj, false); move.asyncExecute(traj); - while(!move.getMoveGroupClient().getState().isDone() && actionClean.load()) loop.sleep(); - if(!actionClean.load()) { - move.stop(); - actionClean.store(true); + isNewPath.store(false); + + while(true){ + + if(move.getMoveGroupClient().getState().isDone()){ + break; + } + + else if(isNewPath.load()){ + move.stop(); + break; + } + + loop.sleep(); + } } diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index 5d5f7e47..c866eb89 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -8,7 +8,7 @@ Panels: - /MotionPlanning1 - /Pose1 Splitter Ratio: 0.7425600290298462 - Tree Height: 241 + Tree Height: 305 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -41,7 +41,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 + - Acceleration_Scaling_Factor: 1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" @@ -189,11 +189,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Robot Alpha: 0.5 + Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true Value: true - Velocity_Scaling_Factor: 0.1 + Velocity_Scaling_Factor: 1 - Alpha: 1 Axes Length: 0.10000000149011612 Axes Radius: 0.029999999329447746 @@ -242,25 +242,25 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4497969150543213 + Pitch: 0.2447967380285263 Target Frame: turntable - Yaw: 5.75994873046875 + Yaw: 3.493121385574341 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 886 + collapsed: true + Height: 1016 Help: collapsed: false - Hide Left Dock: false + Hide Left Dock: true Hide Right Dock: false MotionPlanning: - collapsed: false + collapsed: true MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a20000031cfc0200000007fb000000100044006900730070006c006100790073010000003d00000182000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c5000001940000017d00ffffff000004900000031c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000002a20000039efc0200000007fb000000100044006900730070006c006100790073000000003d000001c2000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000205000001d60000017d00ffffff000007480000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1848 - X: 72 + Width: 1864 + X: 56 Y: 27 From e5f53bc0a18abca94ccff065db2a7332c2150e51 Mon Sep 17 00:00:00 2001 From: Dev Date: Sun, 26 Sep 2021 12:11:14 -0500 Subject: [PATCH 73/85] Removed Yaw(i think and some basic configuration) --- src/wroboarm_21/config/joint_limits.yaml | 6 +++--- src/wroboarm_21/urdf/wroboarm_21.urdf | 26 ++++++++++++------------ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/wroboarm_21/config/joint_limits.yaml b/src/wroboarm_21/config/joint_limits.yaml index 42cd7faf..b07621c4 100644 --- a/src/wroboarm_21/config/joint_limits.yaml +++ b/src/wroboarm_21/config/joint_limits.yaml @@ -2,8 +2,8 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1 +default_acceleration_scaling_factor: 1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] @@ -37,4 +37,4 @@ joint_limits: has_velocity_limits: false max_velocity: 0 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_acceleration: 0 diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf index 27bf0988..a03896a9 100644 --- a/src/wroboarm_21/urdf/wroboarm_21.urdf +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -1,10 +1,10 @@ - - + @@ -28,7 +28,7 @@ - + @@ -55,11 +55,11 @@ - + - + @@ -90,7 +90,7 @@ - + @@ -121,7 +121,7 @@ - + @@ -152,7 +152,7 @@ - + @@ -183,7 +183,7 @@ - + @@ -207,13 +207,13 @@ - + - - + + - + transmission_interface/SimpleTransmission From a1b3f70b322adb155506cd7ced2646b643302e08 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Wed, 29 Sep 2021 20:25:05 -0500 Subject: [PATCH 74/85] atomic reference trigger --- .../src/ArmTeleopLogic.cpp | 55 +++++++++++++++---- 1 file changed, 45 insertions(+), 10 deletions(-) diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index d3e7c88a..665cccec 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -13,11 +13,13 @@ #include #include #include +#include +#include using Std_Bool = const std_msgs::BoolConstPtr&; using Std_Float32 = const std_msgs::Float32ConstPtr&; -std::atomic_bool isNewPath {false}; +std::atomic_bool isNewPath { true }; const tf2::Quaternion WORLD_OFFSET {0, sin(M_PI/2), 0, cos(M_PI/2)}; auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void{ @@ -29,7 +31,7 @@ auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orienta p.pose.orientation = tf2::toMsg(outOrientation); p.header.frame_id = "turntable"; pub.publish(p); - isNewPath.store(true); + // isNewPath.store(true); } auto main(int argc, char** argv) -> int{ @@ -65,6 +67,14 @@ auto main(int argc, char** argv) -> int{ moveit::planning_interface::MoveGroupInterface move("arm"); // move.setPlannerId("RRTStar"); move.setPlanningTime(PLANNING_TIME); + const moveit::core::JointModelGroup* joint_model_group = move.getCurrentState()->getJointModelGroup("arm"); + robot_state::RobotState start_state(*move.getCurrentState()); + + + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools("arm"); + // visual_tools.prompt("Test"); + ros::Rate loop {CLOCK_RATE}; ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", @@ -131,16 +141,27 @@ auto main(int argc, char** argv) -> int{ } })); + ros::Subscriber execPath = np.subscribe("/xbox_test/button/a", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if(msg){ + isNewPath.store(true); + } + })); // transform = move.getCurrentState()->getFrameTransform("odom_combined"); // updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); while(ros::ok()){ + if(!isNewPath.load()){ + loop.sleep(); continue; } - // configure new trajectory - ROS_DEBUG("Hello %s", "World"); + // stop current path + move.stop(); + + // configure target pose geometry_msgs::PoseStamped p {}; p.pose.position.x = x_pos; p.pose.position.y = y_pos; @@ -150,12 +171,27 @@ auto main(int argc, char** argv) -> int{ move.setPoseTarget(p); std::vector waypoints {p.pose}; moveit_msgs::RobotTrajectory traj; - - double discard = move.computeCartesianPath(waypoints, 0.005, 0.0, traj, false); + + // update start state to reflect robot position + move.setStartStateToCurrentState(); + + //plan and execute path + double discard = move.computeCartesianPath(waypoints, 0.005, 0.0, traj, false); //TODO async plannging + + + if(!isNewPath.load()){ + loop.sleep(); + continue; + } + move.asyncExecute(traj); - isNewPath.store(false); - while(true){ + visual_tools.publishTrajectoryLine(traj, joint_model_group); + visual_tools.trigger(); + + isNewPath.store(false); + + while(ros::ok){ if(move.getMoveGroupClient().getState().isDone()){ break; @@ -167,8 +203,7 @@ auto main(int argc, char** argv) -> int{ } loop.sleep(); - - } + } } return 0; From 7a393865bc957900376fd77cdb923e32840e5ea6 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Sun, 3 Oct 2021 16:26:10 -0500 Subject: [PATCH 75/85] fixed cached msg bug - TOFIX: isDone not functioning correctly --- .../src/ArmTeleopLogic.cpp | 26 ++++++++++++------- src/wroboarm_21/urdf/wroboarm_21.urdf | 18 ++++++------- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index 665cccec..0361fa14 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -44,6 +44,7 @@ auto main(int argc, char** argv) -> int{ constexpr float CLOCK_RATE = 2; constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000; constexpr float TRIGGER_PRESSED = 0.5; + constexpr float MINIMUM_PATH_ACCURACY = 0.0; constexpr float STEP_X = 0.001; constexpr float STEP_Y = 0.001; @@ -144,7 +145,8 @@ auto main(int argc, char** argv) -> int{ ros::Subscriber execPath = np.subscribe("/xbox_test/button/a", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Bool msg) -> void { - if(msg){ + if(msg->data){ + std::cout << "[INFO] [" << ros::Time::now() << "]: a button pressed: " << msg << std::endl; isNewPath.store(true); } })); @@ -152,14 +154,18 @@ auto main(int argc, char** argv) -> int{ // updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); while(ros::ok()){ + std::cout << "[INFO] [" << ros::Time::now() << "]: start main loop: " << isNewPath.load() << std::endl; + if(!isNewPath.load()){ + std::cout << "[INFO] [" << ros::Time::now() << "]: no new path" << std::endl; loop.sleep(); continue; } + isNewPath.store(false); // stop current path - move.stop(); + // move.stop(); // configure target pose geometry_msgs::PoseStamped p {}; @@ -178,26 +184,28 @@ auto main(int argc, char** argv) -> int{ //plan and execute path double discard = move.computeCartesianPath(waypoints, 0.005, 0.0, traj, false); //TODO async plannging + std::cout << "[INFO] [" << ros::Time::now() << "]: finished planning" << std::endl; - if(!isNewPath.load()){ + + // check for new paths + if(isNewPath.load() || discard < MINIMUM_PATH_ACCURACY){ loop.sleep(); + std::cout << "[INFO] [" << ros::Time::now() << "]: cancelling computed path" << std::endl; continue; } move.asyncExecute(traj); - visual_tools.publishTrajectoryLine(traj, joint_model_group); - visual_tools.trigger(); - - isNewPath.store(false); - while(ros::ok){ + std::cout << "[INFO] [" << ros::Time::now() << "]: executing path" << std::endl; - if(move.getMoveGroupClient().getState().isDone()){ + if(move.getMoveGroupClient().getState().isDone()){ //TOFIX - move group completes instantly + std::cout << "[INFO] [" << ros::Time::now() << "]: path finished" << std::endl; break; } else if(isNewPath.load()){ + std::cout << "[INFO] [" << ros::Time::now() << "]: path overridden" << std::endl; move.stop(); break; } diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf index a03896a9..66e0e0ff 100644 --- a/src/wroboarm_21/urdf/wroboarm_21.urdf +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -5,7 +5,7 @@ - + @@ -29,7 +29,7 @@ - + @@ -60,7 +60,7 @@ - + @@ -91,7 +91,7 @@ - + @@ -122,7 +122,7 @@ - + @@ -153,7 +153,7 @@ - + @@ -184,7 +184,7 @@ - + @@ -210,8 +210,8 @@ - - + + From e6483e5dba5172f0f834623c55a86bb8e39172db Mon Sep 17 00:00:00 2001 From: Nicholas Date: Sun, 3 Oct 2021 17:17:53 -0500 Subject: [PATCH 76/85] fixed blocked planning, bad state info w/ async plan method --- apt-key.gpg | Bin 0 -> 2537 bytes apt-key.gpg.1 | Bin 0 -> 2537 bytes apt-key.gpg.2 | Bin 0 -> 2537 bytes .../src/ArmTeleopLogic.cpp | 40 ++++-------------- src/wroboarm_21/launch/moveit.rviz | 12 +++--- 5 files changed, 15 insertions(+), 37 deletions(-) create mode 100644 apt-key.gpg create mode 100644 apt-key.gpg.1 create mode 100644 apt-key.gpg.2 diff --git a/apt-key.gpg b/apt-key.gpg new file mode 100644 index 0000000000000000000000000000000000000000..3f0b5a850ba73c4b6d7fe5b206030ee9d0b4da46 GIT binary patch literal 2537 zcmajfc{mj69tZFtYb6Ywv1ebZVJuB(aK<`@5*dapjltLl&DfcP9E`}sG{~;3VJfmD zVak?WwAd1|j4j&@rK{6@o_kOCKF|H<{p0t(fBc^B`+Yv`Ef?9Y>G0XHZ~{`Sx~!T!dveZitA&#vuaeY1DI5>;d|o*d4Lp%{Zqwntwi(+p z&Bu6qBYHxkM)FlF1?NBexq_|FI7ZMRv{MlQfEd*#I?S2~-S?+DRbCJj{x0&zaHoZQ z@XaU&J8W*5{FcKo43vWU4Q!jZW?C$j#Cni=E&V2IAI@Oiq;9Kw&NOJ%(OSBnBrU$X z=14XJdhj?r=p0b}R#OU_@|^`VEqP>`34ZLlyHAxPSo3e(9U6k5dJlF&d^A%%f{gE^`-@y{a6B3mlz$$u#o%;ymD4kW91po+l z;{rT1#VSq7Iw*T{#`DM4<>BIFZ2wHUlknyiqNAb7aNx);hMZ^Dv1wr_Utf{2C<$@n zkIiR35y`vYkSzBnB6-)b!D}YqOMT75_}G;*Q1hZWnT<%Q#`R1GkZ~X*HsIn$wo`p| zT^gMEI@~>dpeU%QD(pf4Qy`&I%PONSbT(s5Ry5`6b~8ghoNp1k+-m|$f9vDz!6H*J zrR>zythuj?q7O&1sBd{d3>Nc0C>8QAJlyS#2f7PfR&5bJE&wZiZ;a8Q6l~sRWJ6n; zHR4Ve19d>;On$7g{rThDCb)k%=|`312YtA#XjbHF6#B)@b+xj)du*(?2i>NI-2wLf zyN%<7wHAhb6)}zCDr43kKAgxF7XU|hDV&ncT(|0kk-)MsydLQ`8bVbr0lFmzoem?y zh~lLREe;mg9nQt*-4MMS;Rh1UB{k5L(U^2IM}nh7$o%#~ zdxKfW6scF)Rl2L-YF+wVn}u&F_rX#|B#rp4T;zOq~9?LMqV;q#mRq`8Bz*7v#~A>RJpb=wEXc_4Q17YN_tpp5LlOx1Ia6k{i}^_ zEzWF0ht(1I@9G=}0QAPW0D?IMz?|Bd_|2GM8?iZ~=YG?V9zSi3wrfks-mx5_CEQ?i zC0_qF%O8Ag+xFXChcVGz;iWG3KHAzEq=hgsJx>J!LdsR3FAYx67? z@MImapIS$CkVzLNeNLgwVT0^O*Iy|3JxeLPg$x4hc;;wt=C0q$mzpI(%#Q^Hrdj%h z$qehpZg2In=r$gI)g6mcVy`x%jc;mw6y#aLQ1nljpe_eo9=(|x`ACaX)t7hp+tB^O z^Pa}ui=y%}QBOAn_+dpH3hT{tudv|@IH4s(@@u8EY(??PS`EZb_TAdCmr%LiBe=PY zpNSvn%D3}Y>abPl+$5I!#YM2sbe+swFi2HG+VyM@>HlAsnW8HOjJO5 zxwnT#BhGb`_Mn&rsweDc3LCsj$`r9KnE-^k8OQ zz8UPXw`vlc&7o=X1WaOU%{f4Cftm)g6&IKHmSQNp?0PaqtErb3nF zmh&;WPtS(B_o8FA&=*-V@0Ay`nGhS^=Gw#zfu>-oCCMjL?RR;slGEC+&aK{Nfu)`l zMZhAk-Hk_krYBJ7{oB@IrNzdqK>#T9d#GUz$E7DeVjO7nAY;^~ROC(c1&ZozbbjO2 z$z+lWa=BTkI8VX6^-N=L#7H^4u;5ZSfihayWH0?5K7*Z22>kogm5S zL>(+Z%2_zc@0D?H)LT`h5iEc*=sR6OzP)$q1-$$$6|>JH#mdf^2Nv3OgrPoHNXH&E zha%o$8Fq?uM0x_DxOB!yl=dDhI0Wu})T;%FgG zPhWJ_daFLl{N$H6nv!ZM`WRWuxt0E%yyJD$!=gBel}daw_~heOrSNwqwfj=_TG@l~ zRl);ujf%0vjed^(rDCB~=$n<(Tn*y9c{9I4`D=9!p%fM20x%jdrWiSQ3Ux#pig$Uk zupT+FjTn6`WE(XqX{s;ZUIbcu$FpJjZn7kB7D%1mwF8M=Pq2%4phoANEG}=$u6nj!VvmSoVC`4jWRTyOvR z7^G0XHZ~{`Sx~!T!dveZitA&#vuaeY1DI5>;d|o*d4Lp%{Zqwntwi(+p z&Bu6qBYHxkM)FlF1?NBexq_|FI7ZMRv{MlQfEd*#I?S2~-S?+DRbCJj{x0&zaHoZQ z@XaU&J8W*5{FcKo43vWU4Q!jZW?C$j#Cni=E&V2IAI@Oiq;9Kw&NOJ%(OSBnBrU$X z=14XJdhj?r=p0b}R#OU_@|^`VEqP>`34ZLlyHAxPSo3e(9U6k5dJlF&d^A%%f{gE^`-@y{a6B3mlz$$u#o%;ymD4kW91po+l z;{rT1#VSq7Iw*T{#`DM4<>BIFZ2wHUlknyiqNAb7aNx);hMZ^Dv1wr_Utf{2C<$@n zkIiR35y`vYkSzBnB6-)b!D}YqOMT75_}G;*Q1hZWnT<%Q#`R1GkZ~X*HsIn$wo`p| zT^gMEI@~>dpeU%QD(pf4Qy`&I%PONSbT(s5Ry5`6b~8ghoNp1k+-m|$f9vDz!6H*J zrR>zythuj?q7O&1sBd{d3>Nc0C>8QAJlyS#2f7PfR&5bJE&wZiZ;a8Q6l~sRWJ6n; zHR4Ve19d>;On$7g{rThDCb)k%=|`312YtA#XjbHF6#B)@b+xj)du*(?2i>NI-2wLf zyN%<7wHAhb6)}zCDr43kKAgxF7XU|hDV&ncT(|0kk-)MsydLQ`8bVbr0lFmzoem?y zh~lLREe;mg9nQt*-4MMS;Rh1UB{k5L(U^2IM}nh7$o%#~ zdxKfW6scF)Rl2L-YF+wVn}u&F_rX#|B#rp4T;zOq~9?LMqV;q#mRq`8Bz*7v#~A>RJpb=wEXc_4Q17YN_tpp5LlOx1Ia6k{i}^_ zEzWF0ht(1I@9G=}0QAPW0D?IMz?|Bd_|2GM8?iZ~=YG?V9zSi3wrfks-mx5_CEQ?i zC0_qF%O8Ag+xFXChcVGz;iWG3KHAzEq=hgsJx>J!LdsR3FAYx67? z@MImapIS$CkVzLNeNLgwVT0^O*Iy|3JxeLPg$x4hc;;wt=C0q$mzpI(%#Q^Hrdj%h z$qehpZg2In=r$gI)g6mcVy`x%jc;mw6y#aLQ1nljpe_eo9=(|x`ACaX)t7hp+tB^O z^Pa}ui=y%}QBOAn_+dpH3hT{tudv|@IH4s(@@u8EY(??PS`EZb_TAdCmr%LiBe=PY zpNSvn%D3}Y>abPl+$5I!#YM2sbe+swFi2HG+VyM@>HlAsnW8HOjJO5 zxwnT#BhGb`_Mn&rsweDc3LCsj$`r9KnE-^k8OQ zz8UPXw`vlc&7o=X1WaOU%{f4Cftm)g6&IKHmSQNp?0PaqtErb3nF zmh&;WPtS(B_o8FA&=*-V@0Ay`nGhS^=Gw#zfu>-oCCMjL?RR;slGEC+&aK{Nfu)`l zMZhAk-Hk_krYBJ7{oB@IrNzdqK>#T9d#GUz$E7DeVjO7nAY;^~ROC(c1&ZozbbjO2 z$z+lWa=BTkI8VX6^-N=L#7H^4u;5ZSfihayWH0?5K7*Z22>kogm5S zL>(+Z%2_zc@0D?H)LT`h5iEc*=sR6OzP)$q1-$$$6|>JH#mdf^2Nv3OgrPoHNXH&E zha%o$8Fq?uM0x_DxOB!yl=dDhI0Wu})T;%FgG zPhWJ_daFLl{N$H6nv!ZM`WRWuxt0E%yyJD$!=gBel}daw_~heOrSNwqwfj=_TG@l~ zRl);ujf%0vjed^(rDCB~=$n<(Tn*y9c{9I4`D=9!p%fM20x%jdrWiSQ3Ux#pig$Uk zupT+FjTn6`WE(XqX{s;ZUIbcu$FpJjZn7kB7D%1mwF8M=Pq2%4phoANEG}=$u6nj!VvmSoVC`4jWRTyOvR z7^G0XHZ~{`Sx~!T!dveZitA&#vuaeY1DI5>;d|o*d4Lp%{Zqwntwi(+p z&Bu6qBYHxkM)FlF1?NBexq_|FI7ZMRv{MlQfEd*#I?S2~-S?+DRbCJj{x0&zaHoZQ z@XaU&J8W*5{FcKo43vWU4Q!jZW?C$j#Cni=E&V2IAI@Oiq;9Kw&NOJ%(OSBnBrU$X z=14XJdhj?r=p0b}R#OU_@|^`VEqP>`34ZLlyHAxPSo3e(9U6k5dJlF&d^A%%f{gE^`-@y{a6B3mlz$$u#o%;ymD4kW91po+l z;{rT1#VSq7Iw*T{#`DM4<>BIFZ2wHUlknyiqNAb7aNx);hMZ^Dv1wr_Utf{2C<$@n zkIiR35y`vYkSzBnB6-)b!D}YqOMT75_}G;*Q1hZWnT<%Q#`R1GkZ~X*HsIn$wo`p| zT^gMEI@~>dpeU%QD(pf4Qy`&I%PONSbT(s5Ry5`6b~8ghoNp1k+-m|$f9vDz!6H*J zrR>zythuj?q7O&1sBd{d3>Nc0C>8QAJlyS#2f7PfR&5bJE&wZiZ;a8Q6l~sRWJ6n; zHR4Ve19d>;On$7g{rThDCb)k%=|`312YtA#XjbHF6#B)@b+xj)du*(?2i>NI-2wLf zyN%<7wHAhb6)}zCDr43kKAgxF7XU|hDV&ncT(|0kk-)MsydLQ`8bVbr0lFmzoem?y zh~lLREe;mg9nQt*-4MMS;Rh1UB{k5L(U^2IM}nh7$o%#~ zdxKfW6scF)Rl2L-YF+wVn}u&F_rX#|B#rp4T;zOq~9?LMqV;q#mRq`8Bz*7v#~A>RJpb=wEXc_4Q17YN_tpp5LlOx1Ia6k{i}^_ zEzWF0ht(1I@9G=}0QAPW0D?IMz?|Bd_|2GM8?iZ~=YG?V9zSi3wrfks-mx5_CEQ?i zC0_qF%O8Ag+xFXChcVGz;iWG3KHAzEq=hgsJx>J!LdsR3FAYx67? z@MImapIS$CkVzLNeNLgwVT0^O*Iy|3JxeLPg$x4hc;;wt=C0q$mzpI(%#Q^Hrdj%h z$qehpZg2In=r$gI)g6mcVy`x%jc;mw6y#aLQ1nljpe_eo9=(|x`ACaX)t7hp+tB^O z^Pa}ui=y%}QBOAn_+dpH3hT{tudv|@IH4s(@@u8EY(??PS`EZb_TAdCmr%LiBe=PY zpNSvn%D3}Y>abPl+$5I!#YM2sbe+swFi2HG+VyM@>HlAsnW8HOjJO5 zxwnT#BhGb`_Mn&rsweDc3LCsj$`r9KnE-^k8OQ zz8UPXw`vlc&7o=X1WaOU%{f4Cftm)g6&IKHmSQNp?0PaqtErb3nF zmh&;WPtS(B_o8FA&=*-V@0Ay`nGhS^=Gw#zfu>-oCCMjL?RR;slGEC+&aK{Nfu)`l zMZhAk-Hk_krYBJ7{oB@IrNzdqK>#T9d#GUz$E7DeVjO7nAY;^~ROC(c1&ZozbbjO2 z$z+lWa=BTkI8VX6^-N=L#7H^4u;5ZSfihayWH0?5K7*Z22>kogm5S zL>(+Z%2_zc@0D?H)LT`h5iEc*=sR6OzP)$q1-$$$6|>JH#mdf^2Nv3OgrPoHNXH&E zha%o$8Fq?uM0x_DxOB!yl=dDhI0Wu})T;%FgG zPhWJ_daFLl{N$H6nv!ZM`WRWuxt0E%yyJD$!=gBel}daw_~heOrSNwqwfj=_TG@l~ zRl);ujf%0vjed^(rDCB~=$n<(Tn*y9c{9I4`D=9!p%fM20x%jdrWiSQ3Ux#pig$Uk zupT+FjTn6`WE(XqX{s;ZUIbcu$FpJjZn7kB7D%1mwF8M=Pq2%4phoANEG}=$u6nj!VvmSoVC`4jWRTyOvR z7^ int{ @@ -71,11 +71,6 @@ auto main(int argc, char** argv) -> int{ const moveit::core::JointModelGroup* joint_model_group = move.getCurrentState()->getJointModelGroup("arm"); robot_state::RobotState start_state(*move.getCurrentState()); - - namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("arm"); - // visual_tools.prompt("Test"); - ros::Rate loop {CLOCK_RATE}; ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", @@ -146,8 +141,7 @@ auto main(int argc, char** argv) -> int{ MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Bool msg) -> void { if(msg->data){ - std::cout << "[INFO] [" << ros::Time::now() << "]: a button pressed: " << msg << std::endl; - isNewPath.store(true); + // isNewPath.store(true); } })); // transform = move.getCurrentState()->getFrameTransform("odom_combined"); @@ -156,16 +150,14 @@ auto main(int argc, char** argv) -> int{ while(ros::ok()){ std::cout << "[INFO] [" << ros::Time::now() << "]: start main loop: " << isNewPath.load() << std::endl; - if(!isNewPath.load()){ - std::cout << "[INFO] [" << ros::Time::now() << "]: no new path" << std::endl; loop.sleep(); continue; } - isNewPath.store(false); // stop current path - // move.stop(); + isNewPath.store(false); + move.stop(); // configure target pose geometry_msgs::PoseStamped p {}; @@ -178,29 +170,15 @@ auto main(int argc, char** argv) -> int{ std::vector waypoints {p.pose}; moveit_msgs::RobotTrajectory traj; - // update start state to reflect robot position - move.setStartStateToCurrentState(); - //plan and execute path - double discard = move.computeCartesianPath(waypoints, 0.005, 0.0, traj, false); //TODO async plannging - - std::cout << "[INFO] [" << ros::Time::now() << "]: finished planning" << std::endl; - - - // check for new paths - if(isNewPath.load() || discard < MINIMUM_PATH_ACCURACY){ - loop.sleep(); - std::cout << "[INFO] [" << ros::Time::now() << "]: cancelling computed path" << std::endl; - continue; - } - - move.asyncExecute(traj); + move.setStartStateToCurrentState(); + move.asyncMove(); while(ros::ok){ - std::cout << "[INFO] [" << ros::Time::now() << "]: executing path" << std::endl; + std::cout << "[INFO] [" << ros::Time::now() << "]: executing path " << std::endl; - if(move.getMoveGroupClient().getState().isDone()){ //TOFIX - move group completes instantly - std::cout << "[INFO] [" << ros::Time::now() << "]: path finished" << std::endl; + if(move.getMoveGroupClient().getState().isDone()){ + std::cout << "[INFO] [" << ros::Time::now() << "]: path finished: " << move.getMoveGroupClient().getState().getText() << std::endl; break; } diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index c866eb89..c4f743bc 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -8,7 +8,7 @@ Panels: - /MotionPlanning1 - /Pose1 Splitter Ratio: 0.7425600290298462 - Tree Height: 305 + Tree Height: 357 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -242,9 +242,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.2447967380285263 + Pitch: 0.6097966432571411 Target Frame: turntable - Yaw: 3.493121385574341 + Yaw: 5.253137111663818 Saved: ~ Window Geometry: Displays: @@ -258,9 +258,9 @@ Window Geometry: collapsed: true MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a20000039efc0200000007fb000000100044006900730070006c006100790073000000003d000001c2000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000205000001d60000017d00ffffff000007480000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000002a20000039efc0200000007fb000000100044006900730070006c006100790073000000003d000001c2000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000205000001d60000017d00ffffff0000045e0000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1864 - X: 56 + Width: 1118 + X: 802 Y: 27 From b718ef549d85a8667b7c6468fb0352c5b2c482fe Mon Sep 17 00:00:00 2001 From: Nicholas Date: Mon, 4 Oct 2021 22:14:36 -0500 Subject: [PATCH 77/85] working dummy link --- apt-key.gpg | Bin 2537 -> 0 bytes apt-key.gpg.1 | Bin 2537 -> 0 bytes apt-key.gpg.2 | Bin 2537 -> 0 bytes reconfig_arm_urdf.sh | 2 ++ .../src/ArmControlSystem.cpp | 3 +- src/wr_control_drive_arm/src/SimpleACS.cpp | 3 +- .../src/ArmTeleopLogic.cpp | 10 +++--- src/wroboarm_21/config/fake_controllers.yaml | 1 + src/wroboarm_21/config/joint_limits.yaml | 5 +++ .../config/joint_names_wroboarm_21.yaml | 2 +- src/wroboarm_21/config/ros_controllers.yaml | 3 +- src/wroboarm_21/config/wroboarm_21.srdf | 3 +- src/wroboarm_21/urdf/wroboarm_21.urdf | 34 +++++++++++++++--- 13 files changed, 52 insertions(+), 14 deletions(-) delete mode 100644 apt-key.gpg delete mode 100644 apt-key.gpg.1 delete mode 100644 apt-key.gpg.2 create mode 100755 reconfig_arm_urdf.sh diff --git a/apt-key.gpg b/apt-key.gpg deleted file mode 100644 index 3f0b5a850ba73c4b6d7fe5b206030ee9d0b4da46..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2537 zcmajfc{mj69tZFtYb6Ywv1ebZVJuB(aK<`@5*dapjltLl&DfcP9E`}sG{~;3VJfmD zVak?WwAd1|j4j&@rK{6@o_kOCKF|H<{p0t(fBc^B`+Yv`Ef?9Y>G0XHZ~{`Sx~!T!dveZitA&#vuaeY1DI5>;d|o*d4Lp%{Zqwntwi(+p z&Bu6qBYHxkM)FlF1?NBexq_|FI7ZMRv{MlQfEd*#I?S2~-S?+DRbCJj{x0&zaHoZQ z@XaU&J8W*5{FcKo43vWU4Q!jZW?C$j#Cni=E&V2IAI@Oiq;9Kw&NOJ%(OSBnBrU$X z=14XJdhj?r=p0b}R#OU_@|^`VEqP>`34ZLlyHAxPSo3e(9U6k5dJlF&d^A%%f{gE^`-@y{a6B3mlz$$u#o%;ymD4kW91po+l z;{rT1#VSq7Iw*T{#`DM4<>BIFZ2wHUlknyiqNAb7aNx);hMZ^Dv1wr_Utf{2C<$@n zkIiR35y`vYkSzBnB6-)b!D}YqOMT75_}G;*Q1hZWnT<%Q#`R1GkZ~X*HsIn$wo`p| zT^gMEI@~>dpeU%QD(pf4Qy`&I%PONSbT(s5Ry5`6b~8ghoNp1k+-m|$f9vDz!6H*J zrR>zythuj?q7O&1sBd{d3>Nc0C>8QAJlyS#2f7PfR&5bJE&wZiZ;a8Q6l~sRWJ6n; zHR4Ve19d>;On$7g{rThDCb)k%=|`312YtA#XjbHF6#B)@b+xj)du*(?2i>NI-2wLf zyN%<7wHAhb6)}zCDr43kKAgxF7XU|hDV&ncT(|0kk-)MsydLQ`8bVbr0lFmzoem?y zh~lLREe;mg9nQt*-4MMS;Rh1UB{k5L(U^2IM}nh7$o%#~ zdxKfW6scF)Rl2L-YF+wVn}u&F_rX#|B#rp4T;zOq~9?LMqV;q#mRq`8Bz*7v#~A>RJpb=wEXc_4Q17YN_tpp5LlOx1Ia6k{i}^_ zEzWF0ht(1I@9G=}0QAPW0D?IMz?|Bd_|2GM8?iZ~=YG?V9zSi3wrfks-mx5_CEQ?i zC0_qF%O8Ag+xFXChcVGz;iWG3KHAzEq=hgsJx>J!LdsR3FAYx67? z@MImapIS$CkVzLNeNLgwVT0^O*Iy|3JxeLPg$x4hc;;wt=C0q$mzpI(%#Q^Hrdj%h z$qehpZg2In=r$gI)g6mcVy`x%jc;mw6y#aLQ1nljpe_eo9=(|x`ACaX)t7hp+tB^O z^Pa}ui=y%}QBOAn_+dpH3hT{tudv|@IH4s(@@u8EY(??PS`EZb_TAdCmr%LiBe=PY zpNSvn%D3}Y>abPl+$5I!#YM2sbe+swFi2HG+VyM@>HlAsnW8HOjJO5 zxwnT#BhGb`_Mn&rsweDc3LCsj$`r9KnE-^k8OQ zz8UPXw`vlc&7o=X1WaOU%{f4Cftm)g6&IKHmSQNp?0PaqtErb3nF zmh&;WPtS(B_o8FA&=*-V@0Ay`nGhS^=Gw#zfu>-oCCMjL?RR;slGEC+&aK{Nfu)`l zMZhAk-Hk_krYBJ7{oB@IrNzdqK>#T9d#GUz$E7DeVjO7nAY;^~ROC(c1&ZozbbjO2 z$z+lWa=BTkI8VX6^-N=L#7H^4u;5ZSfihayWH0?5K7*Z22>kogm5S zL>(+Z%2_zc@0D?H)LT`h5iEc*=sR6OzP)$q1-$$$6|>JH#mdf^2Nv3OgrPoHNXH&E zha%o$8Fq?uM0x_DxOB!yl=dDhI0Wu})T;%FgG zPhWJ_daFLl{N$H6nv!ZM`WRWuxt0E%yyJD$!=gBel}daw_~heOrSNwqwfj=_TG@l~ zRl);ujf%0vjed^(rDCB~=$n<(Tn*y9c{9I4`D=9!p%fM20x%jdrWiSQ3Ux#pig$Uk zupT+FjTn6`WE(XqX{s;ZUIbcu$FpJjZn7kB7D%1mwF8M=Pq2%4phoANEG}=$u6nj!VvmSoVC`4jWRTyOvR z7^G0XHZ~{`Sx~!T!dveZitA&#vuaeY1DI5>;d|o*d4Lp%{Zqwntwi(+p z&Bu6qBYHxkM)FlF1?NBexq_|FI7ZMRv{MlQfEd*#I?S2~-S?+DRbCJj{x0&zaHoZQ z@XaU&J8W*5{FcKo43vWU4Q!jZW?C$j#Cni=E&V2IAI@Oiq;9Kw&NOJ%(OSBnBrU$X z=14XJdhj?r=p0b}R#OU_@|^`VEqP>`34ZLlyHAxPSo3e(9U6k5dJlF&d^A%%f{gE^`-@y{a6B3mlz$$u#o%;ymD4kW91po+l z;{rT1#VSq7Iw*T{#`DM4<>BIFZ2wHUlknyiqNAb7aNx);hMZ^Dv1wr_Utf{2C<$@n zkIiR35y`vYkSzBnB6-)b!D}YqOMT75_}G;*Q1hZWnT<%Q#`R1GkZ~X*HsIn$wo`p| zT^gMEI@~>dpeU%QD(pf4Qy`&I%PONSbT(s5Ry5`6b~8ghoNp1k+-m|$f9vDz!6H*J zrR>zythuj?q7O&1sBd{d3>Nc0C>8QAJlyS#2f7PfR&5bJE&wZiZ;a8Q6l~sRWJ6n; zHR4Ve19d>;On$7g{rThDCb)k%=|`312YtA#XjbHF6#B)@b+xj)du*(?2i>NI-2wLf zyN%<7wHAhb6)}zCDr43kKAgxF7XU|hDV&ncT(|0kk-)MsydLQ`8bVbr0lFmzoem?y zh~lLREe;mg9nQt*-4MMS;Rh1UB{k5L(U^2IM}nh7$o%#~ zdxKfW6scF)Rl2L-YF+wVn}u&F_rX#|B#rp4T;zOq~9?LMqV;q#mRq`8Bz*7v#~A>RJpb=wEXc_4Q17YN_tpp5LlOx1Ia6k{i}^_ zEzWF0ht(1I@9G=}0QAPW0D?IMz?|Bd_|2GM8?iZ~=YG?V9zSi3wrfks-mx5_CEQ?i zC0_qF%O8Ag+xFXChcVGz;iWG3KHAzEq=hgsJx>J!LdsR3FAYx67? z@MImapIS$CkVzLNeNLgwVT0^O*Iy|3JxeLPg$x4hc;;wt=C0q$mzpI(%#Q^Hrdj%h z$qehpZg2In=r$gI)g6mcVy`x%jc;mw6y#aLQ1nljpe_eo9=(|x`ACaX)t7hp+tB^O z^Pa}ui=y%}QBOAn_+dpH3hT{tudv|@IH4s(@@u8EY(??PS`EZb_TAdCmr%LiBe=PY zpNSvn%D3}Y>abPl+$5I!#YM2sbe+swFi2HG+VyM@>HlAsnW8HOjJO5 zxwnT#BhGb`_Mn&rsweDc3LCsj$`r9KnE-^k8OQ zz8UPXw`vlc&7o=X1WaOU%{f4Cftm)g6&IKHmSQNp?0PaqtErb3nF zmh&;WPtS(B_o8FA&=*-V@0Ay`nGhS^=Gw#zfu>-oCCMjL?RR;slGEC+&aK{Nfu)`l zMZhAk-Hk_krYBJ7{oB@IrNzdqK>#T9d#GUz$E7DeVjO7nAY;^~ROC(c1&ZozbbjO2 z$z+lWa=BTkI8VX6^-N=L#7H^4u;5ZSfihayWH0?5K7*Z22>kogm5S zL>(+Z%2_zc@0D?H)LT`h5iEc*=sR6OzP)$q1-$$$6|>JH#mdf^2Nv3OgrPoHNXH&E zha%o$8Fq?uM0x_DxOB!yl=dDhI0Wu})T;%FgG zPhWJ_daFLl{N$H6nv!ZM`WRWuxt0E%yyJD$!=gBel}daw_~heOrSNwqwfj=_TG@l~ zRl);ujf%0vjed^(rDCB~=$n<(Tn*y9c{9I4`D=9!p%fM20x%jdrWiSQ3Ux#pig$Uk zupT+FjTn6`WE(XqX{s;ZUIbcu$FpJjZn7kB7D%1mwF8M=Pq2%4phoANEG}=$u6nj!VvmSoVC`4jWRTyOvR z7^G0XHZ~{`Sx~!T!dveZitA&#vuaeY1DI5>;d|o*d4Lp%{Zqwntwi(+p z&Bu6qBYHxkM)FlF1?NBexq_|FI7ZMRv{MlQfEd*#I?S2~-S?+DRbCJj{x0&zaHoZQ z@XaU&J8W*5{FcKo43vWU4Q!jZW?C$j#Cni=E&V2IAI@Oiq;9Kw&NOJ%(OSBnBrU$X z=14XJdhj?r=p0b}R#OU_@|^`VEqP>`34ZLlyHAxPSo3e(9U6k5dJlF&d^A%%f{gE^`-@y{a6B3mlz$$u#o%;ymD4kW91po+l z;{rT1#VSq7Iw*T{#`DM4<>BIFZ2wHUlknyiqNAb7aNx);hMZ^Dv1wr_Utf{2C<$@n zkIiR35y`vYkSzBnB6-)b!D}YqOMT75_}G;*Q1hZWnT<%Q#`R1GkZ~X*HsIn$wo`p| zT^gMEI@~>dpeU%QD(pf4Qy`&I%PONSbT(s5Ry5`6b~8ghoNp1k+-m|$f9vDz!6H*J zrR>zythuj?q7O&1sBd{d3>Nc0C>8QAJlyS#2f7PfR&5bJE&wZiZ;a8Q6l~sRWJ6n; zHR4Ve19d>;On$7g{rThDCb)k%=|`312YtA#XjbHF6#B)@b+xj)du*(?2i>NI-2wLf zyN%<7wHAhb6)}zCDr43kKAgxF7XU|hDV&ncT(|0kk-)MsydLQ`8bVbr0lFmzoem?y zh~lLREe;mg9nQt*-4MMS;Rh1UB{k5L(U^2IM}nh7$o%#~ zdxKfW6scF)Rl2L-YF+wVn}u&F_rX#|B#rp4T;zOq~9?LMqV;q#mRq`8Bz*7v#~A>RJpb=wEXc_4Q17YN_tpp5LlOx1Ia6k{i}^_ zEzWF0ht(1I@9G=}0QAPW0D?IMz?|Bd_|2GM8?iZ~=YG?V9zSi3wrfks-mx5_CEQ?i zC0_qF%O8Ag+xFXChcVGz;iWG3KHAzEq=hgsJx>J!LdsR3FAYx67? z@MImapIS$CkVzLNeNLgwVT0^O*Iy|3JxeLPg$x4hc;;wt=C0q$mzpI(%#Q^Hrdj%h z$qehpZg2In=r$gI)g6mcVy`x%jc;mw6y#aLQ1nljpe_eo9=(|x`ACaX)t7hp+tB^O z^Pa}ui=y%}QBOAn_+dpH3hT{tudv|@IH4s(@@u8EY(??PS`EZb_TAdCmr%LiBe=PY zpNSvn%D3}Y>abPl+$5I!#YM2sbe+swFi2HG+VyM@>HlAsnW8HOjJO5 zxwnT#BhGb`_Mn&rsweDc3LCsj$`r9KnE-^k8OQ zz8UPXw`vlc&7o=X1WaOU%{f4Cftm)g6&IKHmSQNp?0PaqtErb3nF zmh&;WPtS(B_o8FA&=*-V@0Ay`nGhS^=Gw#zfu>-oCCMjL?RR;slGEC+&aK{Nfu)`l zMZhAk-Hk_krYBJ7{oB@IrNzdqK>#T9d#GUz$E7DeVjO7nAY;^~ROC(c1&ZozbbjO2 z$z+lWa=BTkI8VX6^-N=L#7H^4u;5ZSfihayWH0?5K7*Z22>kogm5S zL>(+Z%2_zc@0D?H)LT`h5iEc*=sR6OzP)$q1-$$$6|>JH#mdf^2Nv3OgrPoHNXH&E zha%o$8Fq?uM0x_DxOB!yl=dDhI0Wu})T;%FgG zPhWJ_daFLl{N$H6nv!ZM`WRWuxt0E%yyJD$!=gBel}daw_~heOrSNwqwfj=_TG@l~ zRl);ujf%0vjed^(rDCB~=$n<(Tn*y9c{9I4`D=9!p%fM20x%jdrWiSQ3Ux#pig$Uk zupT+FjTn6`WE(XqX{s;ZUIbcu$FpJjZn7kB7D%1mwF8M=Pq2%4phoANEG}=$u6nj!VvmSoVC`4jWRTyOvR z7^("/control/arm_joint_states", 1000); diff --git a/src/wr_control_drive_arm/src/SimpleACS.cpp b/src/wr_control_drive_arm/src/SimpleACS.cpp index 1c805c28..7b0b5e4d 100644 --- a/src/wr_control_drive_arm/src/SimpleACS.cpp +++ b/src/wr_control_drive_arm/src/SimpleACS.cpp @@ -5,7 +5,7 @@ #include #include "ArmMotor.hpp" -ArmMotor* motors[6]; +ArmMotor* motors[7]; ros::Publisher jointStatePublisher; typedef actionlib::SimpleActionServer Server; @@ -40,6 +40,7 @@ int main(int argc, char** argv) motors[3] = new ArmMotor("link4_joint", 1, 1, &n); motors[4] = new ArmMotor("link5_joint", 2, 0, &n); motors[5] = new ArmMotor("link6_joint", 2, 1, &n); + motors[6] = new ArmMotor("link7_joint", 3, 0, &n); jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index dfbf61a6..e3015a64 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -31,7 +31,7 @@ auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orienta p.pose.orientation = tf2::toMsg(outOrientation); p.header.frame_id = "turntable"; pub.publish(p); - isNewPath.store(true); + // isNewPath.store(true); } auto main(int argc, char** argv) -> int{ @@ -76,7 +76,6 @@ auto main(int argc, char** argv) -> int{ ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", MESSAGE_QUEUE_LENGTH); - ros::Subscriber yAxis = np.subscribe("/xbox_test/axis/pov_y", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { @@ -85,6 +84,7 @@ auto main(int argc, char** argv) -> int{ updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); } })); + ros::Subscriber xAxis = np.subscribe("/xbox_test/axis/pov_x", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { @@ -93,6 +93,7 @@ auto main(int argc, char** argv) -> int{ updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); } })); + ros::Subscriber zUp = np.subscribe("/xbox_test/button/shoulder_l", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Bool msg) -> void { @@ -101,6 +102,7 @@ auto main(int argc, char** argv) -> int{ updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); } })); + ros::Subscriber zDown = np.subscribe("/xbox_test/axis/trigger_left", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { @@ -141,14 +143,14 @@ auto main(int argc, char** argv) -> int{ MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Bool msg) -> void { if(msg->data){ - // isNewPath.store(true); + isNewPath.store(true); } })); + // transform = move.getCurrentState()->getFrameTransform("odom_combined"); // updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); while(ros::ok()){ - std::cout << "[INFO] [" << ros::Time::now() << "]: start main loop: " << isNewPath.load() << std::endl; if(!isNewPath.load()){ loop.sleep(); diff --git a/src/wroboarm_21/config/fake_controllers.yaml b/src/wroboarm_21/config/fake_controllers.yaml index c357a98e..e296f940 100644 --- a/src/wroboarm_21/config/fake_controllers.yaml +++ b/src/wroboarm_21/config/fake_controllers.yaml @@ -7,6 +7,7 @@ controller_list: - link4_joint - link5_joint - link6_joint + - link7_joint initial: # Define initial robot poses. - group: arm # pose: home diff --git a/src/wroboarm_21/config/joint_limits.yaml b/src/wroboarm_21/config/joint_limits.yaml index b07621c4..18062faa 100644 --- a/src/wroboarm_21/config/joint_limits.yaml +++ b/src/wroboarm_21/config/joint_limits.yaml @@ -38,3 +38,8 @@ joint_limits: max_velocity: 0 has_acceleration_limits: false max_acceleration: 0 + link7_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/src/wroboarm_21/config/joint_names_wroboarm_21.yaml b/src/wroboarm_21/config/joint_names_wroboarm_21.yaml index 41ccc5d4..fbfbe031 100644 --- a/src/wroboarm_21/config/joint_names_wroboarm_21.yaml +++ b/src/wroboarm_21/config/joint_names_wroboarm_21.yaml @@ -1 +1 @@ -controller_joint_names: ['', 'link1_joint', 'link2_joint', 'link3_joint', 'link4_joint', 'link5_joint', 'link6_joint', ] +controller_joint_names: ['', 'link1_joint', 'link2_joint', 'link3_joint', 'link4_joint', 'link5_joint', 'link6_joint', 'link7_joint', ] diff --git a/src/wroboarm_21/config/ros_controllers.yaml b/src/wroboarm_21/config/ros_controllers.yaml index dc77ec42..fe7ecd81 100644 --- a/src/wroboarm_21/config/ros_controllers.yaml +++ b/src/wroboarm_21/config/ros_controllers.yaml @@ -15,6 +15,7 @@ hardware_interface: - link4_joint - link5_joint - link6_joint + - link7_joint sim_control_mode: 1 # 0: position, 1: velocity # Publish all joint states # Creates the /joint_states topic necessary in ROS @@ -32,4 +33,4 @@ controller_list: - link3_joint - link4_joint - link5_joint - - link6_joint \ No newline at end of file + - link7_joint \ No newline at end of file diff --git a/src/wroboarm_21/config/wroboarm_21.srdf b/src/wroboarm_21/config/wroboarm_21.srdf index 4b9ab407..cef5e442 100644 --- a/src/wroboarm_21/config/wroboarm_21.srdf +++ b/src/wroboarm_21/config/wroboarm_21.srdf @@ -17,9 +17,10 @@ + - + diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf index 66e0e0ff..cfc18411 100644 --- a/src/wroboarm_21/urdf/wroboarm_21.urdf +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -87,7 +87,7 @@ - + @@ -118,7 +118,7 @@ - + @@ -182,6 +182,20 @@ + + + + + + + + + + + + + + @@ -206,15 +220,15 @@ - + - + - + transmission_interface/SimpleTransmission hardware_interface/EffortJointInterface @@ -274,6 +288,16 @@ 1 + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + / From def2de5ab6a250812dd6d03b5ab13324dbd093f2 Mon Sep 17 00:00:00 2001 From: Dev Date: Mon, 11 Oct 2021 19:16:18 -0500 Subject: [PATCH 78/85] Added link 7 to some places and chmod file --- make_py_nodes_755.py | 13 +++++++++++++ src/wroboarm_21/config/ros_controllers.yaml | 3 ++- src/wroboarm_21/urdf/wroboarm_21.urdf | 4 ++-- 3 files changed, 17 insertions(+), 3 deletions(-) create mode 100755 make_py_nodes_755.py diff --git a/make_py_nodes_755.py b/make_py_nodes_755.py new file mode 100755 index 00000000..a9f14df3 --- /dev/null +++ b/make_py_nodes_755.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python3 + +import os + +for pkg_name in os.listdir('src'): + pkg_dir = f'src/{pkg_name}' + if os.path.isdir(pkg_dir): + nodes_dir = f'{pkg_dir}/nodes' + if os.path.isdir(nodes_dir): + for node_name in os.listdir(nodes_dir): + node_path = f'{nodes_dir}/{node_name}' + print(node_path) + os.chmod(node_path, 0o755) diff --git a/src/wroboarm_21/config/ros_controllers.yaml b/src/wroboarm_21/config/ros_controllers.yaml index fe7ecd81..ebdad9d2 100644 --- a/src/wroboarm_21/config/ros_controllers.yaml +++ b/src/wroboarm_21/config/ros_controllers.yaml @@ -33,4 +33,5 @@ controller_list: - link3_joint - link4_joint - link5_joint - - link7_joint \ No newline at end of file + - link6_joint + - link7_joint diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf index cfc18411..d405e75f 100644 --- a/src/wroboarm_21/urdf/wroboarm_21.urdf +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -290,10 +290,10 @@ transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface - + hardware_interface/EffortJointInterface 1 From 27d956a081d963783456d860a506f150351e2b80 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Sun, 17 Oct 2021 17:52:52 -0500 Subject: [PATCH 79/85] dummy axis fully integrated in IK --- src/wr_control_drive_arm/config/arm_motor_PID.yaml | 9 +++++++++ src/wr_control_drive_arm/src/SimpleACS.cpp | 8 ++++---- src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp | 2 +- src/wroboarm_21/config/ros_controllers.yaml | 1 + src/wroboarm_21/launch/moveit.rviz | 10 ++++++++++ 5 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/wr_control_drive_arm/config/arm_motor_PID.yaml b/src/wr_control_drive_arm/config/arm_motor_PID.yaml index bf359534..2e6a03e7 100644 --- a/src/wr_control_drive_arm/config/arm_motor_PID.yaml +++ b/src/wr_control_drive_arm/config/arm_motor_PID.yaml @@ -52,4 +52,13 @@ controllers: I: 0 D: 0 min: -1 + max: 1 + + - setpointTopic: "/control/arm/30/setpoint" + feedbackTopic: "/control/arm/30/feedback" + outputTopic: "/control/arm/30/output" + P: 30 + I: 0 + D: 0 + min: -1 max: 1 \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleACS.cpp b/src/wr_control_drive_arm/src/SimpleACS.cpp index 7b0b5e4d..8417a880 100644 --- a/src/wr_control_drive_arm/src/SimpleACS.cpp +++ b/src/wr_control_drive_arm/src/SimpleACS.cpp @@ -19,10 +19,10 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server int i = 0; ros::Rate sleeper(50); while(i++ < 100){ - std::cout<getEncoderCounts()<getPower()<getEncoderCounts()<getPower()<setSucceeded(); diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index e3015a64..1f29acd5 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -177,7 +177,7 @@ auto main(int argc, char** argv) -> int{ move.asyncMove(); while(ros::ok){ - std::cout << "[INFO] [" << ros::Time::now() << "]: executing path " << std::endl; + // std::cout << "[INFO] [" << ros::Time::now() << "]: executing path " << std::endl; if(move.getMoveGroupClient().getState().isDone()){ std::cout << "[INFO] [" << ros::Time::now() << "]: path finished: " << move.getMoveGroupClient().getState().getText() << std::endl; diff --git a/src/wroboarm_21/config/ros_controllers.yaml b/src/wroboarm_21/config/ros_controllers.yaml index fe7ecd81..799a9193 100644 --- a/src/wroboarm_21/config/ros_controllers.yaml +++ b/src/wroboarm_21/config/ros_controllers.yaml @@ -33,4 +33,5 @@ controller_list: - link3_joint - link4_joint - link5_joint + - link6_joint - link7_joint \ No newline at end of file diff --git a/src/wroboarm_21/launch/moveit.rviz b/src/wroboarm_21/launch/moveit.rviz index c4f743bc..87a9b996 100644 --- a/src/wroboarm_21/launch/moveit.rviz +++ b/src/wroboarm_21/launch/moveit.rviz @@ -74,6 +74,11 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + dummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true elbow: Alpha: 1 Show Axes: false @@ -154,6 +159,11 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + dummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true elbow: Alpha: 1 Show Axes: false From 29c53ad29b6407c0d9e1ef4a530f67702d0b68c0 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Sun, 17 Oct 2021 20:35:41 -0500 Subject: [PATCH 80/85] c++ pointers are dumb --- .../src/ArmControlSystem.cpp | 180 +++++++++++------- 1 file changed, 116 insertions(+), 64 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 30ef0562..f3653339 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include "ArmMotor.hpp" /** @@ -25,6 +26,47 @@ ros::Publisher jointStatePublisher; */ typedef actionlib::SimpleActionServer Server; +/** + * @brief Converts position space to encoder space + * + * @param positionTargets the pitch and roll positions of the differential joint + * @return the left and right position targets for the joint motors + */ +std::vector getEncoderSpace(std::vector positionTargets){ + std::vector encoderTargets; + + // TODO: real matrix conversion, integrate joint into wroboclaw mock.py + encoderTargets[0] = positionTargets[0]; + encoderTargets[1] = positionTargets[1]; + + return encoderTargets; +} + + +/** + * @brief sets a target position and pulls info from motor + * + * @param names a vector with motor joint names + * @param positions a vector with positions in radians + * @param target the target radian value + * @param motor a pointer to the motor + * @return if the motor has reached its target + */ +bool configMotorTarget(std::vector names, std::vector positions, double target, ArmMotor* motor){ + // Each motor should run to its respective target position at a fixed speed + // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters + // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)runToTarget(target, 0); + // Push the current motor name and position data to the Joint State data tracking list + names.push_back(motor->getMotorName()); + positions.push_back(motor->getRads()); + // The position has only finished if every motor is STOPped + return motor->getMotorState() == MotorState::STOP; +} + /** * @brief Perform the given action as interpreted as moving the arm joints to specified positions * @@ -32,71 +74,79 @@ typedef actionlib::SimpleActionServer * @param as The Action Server this is occuring on */ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { - // For each point in the trajectory execution sequence... - for(int i = 0; i < goal->trajectory.points.size(); i++){ - // Capture the current goal for easy reference - trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; - - // Track whether or not the current position is done - bool hasPositionFinished = false; - // Capture the names of the motors - std::vector names; - // Capture the positions of the motors - std::vector positions; - // Keep max loop rate at 50 Hz - ros::Rate loop(50); - - // While the current position is not complete yet... - while(!hasPositionFinished){ - // Assume the current action is done until proven otherwise - hasPositionFinished = true; - // Create the Joint State message for the current update cycle - sensor_msgs::JointState js_msg; - - // Clear the list of motor names and position data - names.clear(); - positions.clear(); - - double velMax = 0; - // For each motor specified in the currTargetPosition... - for(int j = 0; j < currTargetPosition.positions.size(); j++){ - // Each motor should run to its respective target position at a fixed speed - // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters - // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)runToTarget(currTargetPosition.positions[j], 0);//currPower); - // The position has only finished if every motor is STOPped - hasPositionFinished &= motors[j]->getMotorState() == MotorState::STOP; - // Push the current motor name and position data to the Joint State data tracking list - names.push_back(motors[j]->getMotorName()); - positions.push_back(motors[j]->getRads()); - - // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power - std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()<setSucceeded(); + std::cout << "execute trajectory" << std::endl; + // For each point in the trajectory execution sequence... + for(int i = 0; i < goal->trajectory.points.size(); i++){ + // Capture the current goal for easy reference + trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; + + // Track whether or not the current position is done + bool hasPositionFinished = false; + // Capture the names of the motors + std::vector names; + // Capture the positions of the motors + std::vector positions; + // the target space of the differential joint + std::vector differentialPositionTargets; + // the target space of the differential joint + std::vector differentialEncoderTargets; + // Keep max loop rate at 50 Hz + ros::Rate loop(50); + + // While the current position is not complete yet... + while(!hasPositionFinished){ + // Assume the current action is done until proven otherwise + hasPositionFinished = true; + // Create the Joint State message for the current update cycle + sensor_msgs::JointState js_msg; + + // Clear the list of motor names and position data + names.clear(); + positions.clear(); + + double velMax = 0; + // For each motor specified in the currTargetPosition... + for(int j = 0; j < currTargetPosition.positions.size(); j++){ + + hasPositionFinished &= configMotorTarget(names, positions, currTargetPosition.positions[j], motors[j]); + + // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power + // std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()<setSucceeded(); } + + /** * @brief The main executable method of the node. Starts the ROS node and the Action Server for processing Arm Control commands * @@ -106,6 +156,8 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server */ int main(int argc, char** argv) { + std::cout << "init ArmControlSystem" << std::endl; + // Initialize the current node as ArmControlSystem ros::init(argc, argv, "ArmControlSystem"); // Create the NodeHandle to the current ROS node @@ -118,7 +170,7 @@ int main(int argc, char** argv) motors[3] = new ArmMotor("link4_joint", 1, 1, &n); motors[4] = new ArmMotor("link5_joint", 2, 0, &n); motors[5] = new ArmMotor("link6_joint", 2, 1, &n); - motors[5] = new ArmMotor("link7_joint", 3, 0, &n); + motors[6] = new ArmMotor("link7_joint", 3, 0, &n); // Initialize the Joint State Data Publisher jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); From 1f8bbde635254b16bc46f2b582989463bf442794 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Mon, 18 Oct 2021 15:14:57 -0500 Subject: [PATCH 81/85] added redundant linear transformation function --- .../src/ArmControlSystem.cpp | 157 ++++++++---------- 1 file changed, 71 insertions(+), 86 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index f3653339..daf05eba 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include "ArmMotor.hpp" /** @@ -27,22 +27,22 @@ ros::Publisher jointStatePublisher; typedef actionlib::SimpleActionServer Server; /** - * @brief Converts position space to encoder space + * @brief converts a roll and pitch to joint motor positions * - * @param positionTargets the pitch and roll positions of the differential joint - * @return the left and right position targets for the joint motors + * @param roll the roll of the joint in radians + * @param pitch the pitch of the joint in radians + * @return a 2D vector with motor positions in radians */ -std::vector getEncoderSpace(std::vector positionTargets){ - std::vector encoderTargets; +std::vector convertJointSpacetoEncoderSpace(double roll, double pitch){ + std::vector encoderSpace(2); - // TODO: real matrix conversion, integrate joint into wroboclaw mock.py - encoderTargets[0] = positionTargets[0]; - encoderTargets[1] = positionTargets[1]; + //TODO: add matrix transformation, implement joint in mock.py + encoderSpace[0] = roll; + encoderSpace[1] = pitch; - return encoderTargets; + return encoderSpace; } - /** * @brief sets a target position and pulls info from motor * @@ -52,7 +52,7 @@ std::vector getEncoderSpace(std::vector positionTargets){ * @param motor a pointer to the motor * @return if the motor has reached its target */ -bool configMotorTarget(std::vector names, std::vector positions, double target, ArmMotor* motor){ +bool configMotorTarget(std::vector* names, std::vector* positions, double target, float velocity, ArmMotor* motor){ // Each motor should run to its respective target position at a fixed speed // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a) names, std::vector posit motor->runToTarget(target, 0); // Push the current motor name and position data to the Joint State data tracking list - names.push_back(motor->getMotorName()); - positions.push_back(motor->getRads()); + names->push_back(motor->getMotorName()); + positions->push_back(motor->getRads()); // The position has only finished if every motor is STOPped return motor->getMotorState() == MotorState::STOP; } @@ -74,79 +74,66 @@ bool configMotorTarget(std::vector names, std::vector posit * @param as The Action Server this is occuring on */ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { - std::cout << "execute trajectory" << std::endl; - // For each point in the trajectory execution sequence... - for(int i = 0; i < goal->trajectory.points.size(); i++){ - // Capture the current goal for easy reference - trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; - - // Track whether or not the current position is done - bool hasPositionFinished = false; - // Capture the names of the motors - std::vector names; - // Capture the positions of the motors - std::vector positions; - // the target space of the differential joint - std::vector differentialPositionTargets; - // the target space of the differential joint - std::vector differentialEncoderTargets; - // Keep max loop rate at 50 Hz - ros::Rate loop(50); - - // While the current position is not complete yet... - while(!hasPositionFinished){ - // Assume the current action is done until proven otherwise - hasPositionFinished = true; - // Create the Joint State message for the current update cycle - sensor_msgs::JointState js_msg; - - // Clear the list of motor names and position data - names.clear(); - positions.clear(); - - double velMax = 0; - // For each motor specified in the currTargetPosition... - for(int j = 0; j < currTargetPosition.positions.size(); j++){ - - hasPositionFinished &= configMotorTarget(names, positions, currTargetPosition.positions[j], motors[j]); - - // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power - // std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()<trajectory.points.size(); i++){ + // Capture the current goal for easy reference + trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; + + // Track whether or not the current position is done + bool hasPositionFinished = false; + // Capture the names of the motors + std::vector names; + // Capture the positions of the motors + std::vector positions; + // Keep max loop rate at 50 Hz + ros::Rate loop(200); + + // While the current position is not complete yet... + while(!hasPositionFinished){ + // Assume the current action is done until proven otherwise + hasPositionFinished = true; + // Create the Joint State message for the current update cycle + sensor_msgs::JointState js_msg; + + // Clear the list of motor names and position data + names.clear(); + positions.clear(); + + double velMax = 0; + // For each motor specified in the currTargetPosition... + for(int j = 0; j < currTargetPosition.positions.size()-2; j++){ + hasPositionFinished &= configMotorTarget(&names, &positions, currTargetPosition.positions[j], 0, motors[j]); + // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power + std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()< motorTargets = convertJointSpacetoEncoderSpace(currTargetPosition.positions[5], currTargetPosition.positions[6]); + hasPositionFinished &= configMotorTarget(&names, &positions, motorTargets[0], 0, motors[5]); + hasPositionFinished &= configMotorTarget(&names, &positions, motorTargets[1], 0, motors[6]); + // DEBUGGING OUTPUT: Print a divider line for cleanliness - // std::cout<setSucceeded(); + std::cout<setSucceeded(); } - - /** * @brief The main executable method of the node. Starts the ROS node and the Action Server for processing Arm Control commands * @@ -156,8 +143,6 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server */ int main(int argc, char** argv) { - std::cout << "init ArmControlSystem" << std::endl; - // Initialize the current node as ArmControlSystem ros::init(argc, argv, "ArmControlSystem"); // Create the NodeHandle to the current ROS node From ca85dcf9917972474f4b7f3868ddc42090772215 Mon Sep 17 00:00:00 2001 From: Nicholas Date: Wed, 20 Oct 2021 19:12:40 -0500 Subject: [PATCH 82/85] add differential transmission --- .../src/ArmControlSystem.cpp | 2 +- src/wroboarm_21/config/joint_limits.yaml | 4 ++-- src/wroboarm_21/urdf/wroboarm_21.urdf | 22 +++++++++---------- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index daf05eba..57f43519 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -109,7 +109,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server std::cout<getPower()< motorTargets = convertJointSpacetoEncoderSpace(currTargetPosition.positions[5], currTargetPosition.positions[6]); hasPositionFinished &= configMotorTarget(&names, &positions, motorTargets[0], 0, motors[5]); hasPositionFinished &= configMotorTarget(&names, &positions, motorTargets[1], 0, motors[6]); diff --git a/src/wroboarm_21/config/joint_limits.yaml b/src/wroboarm_21/config/joint_limits.yaml index 18062faa..be8e07da 100644 --- a/src/wroboarm_21/config/joint_limits.yaml +++ b/src/wroboarm_21/config/joint_limits.yaml @@ -2,8 +2,8 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 1 -default_acceleration_scaling_factor: 1 +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] diff --git a/src/wroboarm_21/urdf/wroboarm_21.urdf b/src/wroboarm_21/urdf/wroboarm_21.urdf index d405e75f..b76d719e 100644 --- a/src/wroboarm_21/urdf/wroboarm_21.urdf +++ b/src/wroboarm_21/urdf/wroboarm_21.urdf @@ -278,26 +278,26 @@ 1 - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - + + transmission_interface/DifferentialTransmission + actuator1 hardware_interface/EffortJointInterface 1 - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - + actuator2 hardware_interface/EffortJointInterface 1 + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + / From 9034e99083705023438d7d2108f129aee4e5eedc Mon Sep 17 00:00:00 2001 From: Arthur Date: Thu, 21 Oct 2021 18:15:57 -0500 Subject: [PATCH 83/85] Using ssh instead of git to pull. --- project.json | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/project.json b/project.json index 0bf78808..8dd30293 100644 --- a/project.json +++ b/project.json @@ -2,28 +2,28 @@ "deps": [ { "name": "wroboclaw", - "repo": "https://github.com/WisconsinRobotics/wroboclaw.git", + "repo": "ssh://git@github.com/WisconsinRobotics/wroboclaw.git", "build": { "provider": "pip" } }, { "name": "wready", - "repo": "https://github.com/WisconsinRobotics/wready.git", + "repo": "ssh://git@github.com/WisconsinRobotics/wready.git", "build": { "provider": "pip" } }, { "name": "wreadinput", - "repo": "https://github.com/WisconsinRobotics/wreadinput.git", + "repo": "ssh://git@github.com/WisconsinRobotics/wreadinput.git", "build": { "provider": "pip" } }, { "name": "wresponsecontrol", - "repo": "https://github.com/WisconsinRobotics/wresponsecontrol.git", + "repo": "ssh://git@github.com/WisconsinRobotics/wresponsecontrol.git", "build": { "provider": "noop" } From 2e1544be5e3209eb8c79fc7e93405b9dae981130 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 25 Oct 2021 19:33:32 -0500 Subject: [PATCH 84/85] Working model. --- src/wr_entry_point/launch/test/test_arm.launch | 4 ++-- .../launch/{test_arm.launch => test_arm_OLD.launch} | 0 src/wr_logic_teleop_arm/nodes/watchdog.py | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename src/wr_entry_point/launch/{test_arm.launch => test_arm_OLD.launch} (100%) mode change 100644 => 100755 src/wr_logic_teleop_arm/nodes/watchdog.py diff --git a/src/wr_entry_point/launch/test/test_arm.launch b/src/wr_entry_point/launch/test/test_arm.launch index d834e3de..16dd0cf9 100644 --- a/src/wr_entry_point/launch/test/test_arm.launch +++ b/src/wr_entry_point/launch/test/test_arm.launch @@ -8,7 +8,7 @@ - + - + diff --git a/src/wr_entry_point/launch/test_arm.launch b/src/wr_entry_point/launch/test_arm_OLD.launch similarity index 100% rename from src/wr_entry_point/launch/test_arm.launch rename to src/wr_entry_point/launch/test_arm_OLD.launch diff --git a/src/wr_logic_teleop_arm/nodes/watchdog.py b/src/wr_logic_teleop_arm/nodes/watchdog.py old mode 100644 new mode 100755 From 03ae56ca6cb878da7c61f9c6e10f983c7e9795bc Mon Sep 17 00:00:00 2001 From: Ben Nowotny <44074469+bennowotny@users.noreply.github.com> Date: Thu, 7 Apr 2022 20:11:05 -0500 Subject: [PATCH 85/85] Dev/arm stall (#19) (#20) * header for differential joint class * joint progress: non functional * simple joint compiles, does not work * differential joint - compiles - untested * position handoff is working, velocity untested * gpp is very unhappy * almost * added checks for stall in ArmMotor and up * debugging * debugging: fixed issues with incompatible variables, added TODO in execute method of ArmControlSystem * Updated implementation incorporating actual motor current publishers * Began services * More service/client implementation and comments * Dev/differential joint (#18) * compiles * execution working, error with wrist matrix * fully functional - need mocked writst logic * Dev/new urdf (#17) * model working, trajectory data if wrong * working :) * good joint limits * controller mapping changes * simple joint abstraction workging with new model * differentail joint working * fixed entry point launch file conflicts * fixed teleop bug and combined arm test into one launch file * Arm viz (#16) * added motor specfic bound + offset for arm * mocked roboclaws working with new encoders * fixed encToRad * ready to test * fixed launch files * fixed diff joint encoder reading * Config file and setup changes due allow robot to run normally, and then add visualization on top. There are pending logical and setup errors in at minimum ArmControlSystem.cpp that must be fixed prior to testing. * debugging for publish() * Updating `corrMod` implementation. * lint * lint-b-gone * liiiiiiiiiiiint * Tuning/preliminary homing updates. * Parameterizing offset/ratio parameters for the arm. Initializing default position to be offset ("home"). * Further tuning updates. Co-authored-by: Nicholas Underwood Co-authored-by: WR-BaseStation Co-authored-by: Arthur Wang Co-authored-by: Nicholas Co-authored-by: Nicholas Underwood Co-authored-by: WR-BaseStation Co-authored-by: Arthur Wang Co-authored-by: Nicholas Co-authored-by: Nicholas Underwood Co-authored-by: WR-BaseStation Co-authored-by: Arthur Wang Co-authored-by: Nicholas Co-authored-by: Arthur Wang Co-authored-by: Nicholas Underwood Co-authored-by: WR-BaseStation Co-authored-by: Nicholas Co-authored-by: Arthur Wang Co-authored-by: Nicholas Underwood Co-authored-by: WR-BaseStation --- .gitignore | 7 +- src/arm_assembly/CMakeLists.txt | 14 + .../config/joint_names_arm_assembly.yaml | 1 + src/arm_assembly/export.log | 566 ++++++++++++++++++ src/arm_assembly/launch/display.launch | 20 + src/arm_assembly/launch/gazebo.launch | 20 + src/arm_assembly/meshes/base_link.STL | Bin 0 -> 11984 bytes src/arm_assembly/meshes/forarm_link.STL | Bin 0 -> 19984 bytes src/arm_assembly/meshes/manipulator_link.STL | Bin 0 -> 54284 bytes src/arm_assembly/meshes/shoulderBase_link.STL | Bin 0 -> 53584 bytes src/arm_assembly/meshes/upperArm_link.STL | Bin 0 -> 25984 bytes src/arm_assembly/meshes/wirstBase_link.STL | Bin 0 -> 12484 bytes src/arm_assembly/meshes/wrist_link.STL | Bin 0 -> 50884 bytes src/arm_assembly/package.xml | 21 + src/arm_assembly/urdf/arm_assembly.csv | 8 + src/arm_assembly/urdf/arm_assembly.urdf | 282 +++++++++ src/wr_control_drive_arm/CMakeLists.txt | 6 +- .../config/encoder_parameters.yaml | 19 + src/wr_control_drive_arm/launch/std.launch | 6 +- .../src/AbstractJoint.cpp | 39 ++ .../src/AbstractJoint.hpp | 43 ++ .../src/ArmControlSystem.cpp | 243 +++++--- src/wr_control_drive_arm/src/ArmMotor.cpp | 143 +++-- src/wr_control_drive_arm/src/ArmMotor.hpp | 82 ++- .../src/DifferentialJoint.cpp | 122 ++++ .../src/DifferentialJoint.hpp | 48 ++ src/wr_control_drive_arm/src/SimpleACS.cpp | 51 -- src/wr_control_drive_arm/src/SimpleJoint.cpp | 48 ++ src/wr_control_drive_arm/src/SimpleJoint.hpp | 28 + .../launch/comp/comp_arm.launch | 2 +- src/wr_entry_point/launch/mock_arm.launch | 5 + .../launch/test/test_arm.launch | 10 +- src/wr_entry_point/launch/test/vis_arm.launch | 22 + src/wr_hsi_roboclaw/config/roboclaw_enc.yaml | 16 +- .../src/ArmTeleopLogic.cpp | 143 +++-- src/wroboarm_22/.setup_assistant | 11 + src/wroboarm_22/CMakeLists.txt | 14 + src/wroboarm_22/config/arm_assembly.srdf | 43 ++ src/wroboarm_22/config/cartesian_limits.yaml | 5 + src/wroboarm_22/config/chomp_planning.yaml | 18 + src/wroboarm_22/config/fake_controllers.yaml | 15 + .../config/gazebo_controllers.yaml | 4 + src/wroboarm_22/config/joint_limits.yaml | 52 ++ .../config/joint_names_wroboarm_22.yaml | 1 + src/wroboarm_22/config/kinematics.yaml | 4 + src/wroboarm_22/config/ompl_planning.yaml | 155 +++++ src/wroboarm_22/config/ros_controllers.yaml | 35 ++ src/wroboarm_22/config/sensors_3d.yaml | 10 + .../config/simple_moveit_controllers.yaml | 23 + src/wroboarm_22/config/wroboarm_22.srdf | 34 ++ src/wroboarm_22/export.log | 566 ++++++++++++++++++ ..._assembly_moveit_sensor_manager.launch.xml | 3 + .../launch/chomp_planning_pipeline.launch.xml | 25 + .../launch/default_warehouse_db.launch | 15 + src/wroboarm_22/launch/demo.launch | 67 +++ src/wroboarm_22/launch/demo_gazebo.launch | 73 +++ src/wroboarm_22/launch/demo_test.launch | 67 +++ src/wroboarm_22/launch/display.launch | 20 + .../fake_moveit_controller_manager.launch.xml | 12 + src/wroboarm_22/launch/gazebo copy.launch | 23 + src/wroboarm_22/launch/gazebo.launch | 20 + .../launch/joystick_control.launch | 17 + src/wroboarm_22/launch/move_group.launch | 104 ++++ src/wroboarm_22/launch/moveit.rviz | 338 +++++++++++ src/wroboarm_22/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 28 + ...otion_planner_planning_pipeline.launch.xml | 19 + .../launch/planning_context.launch | 26 + .../launch/planning_pipeline.launch.xml | 10 + ...ntrol_moveit_controller_manager.launch.xml | 4 + src/wroboarm_22/launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 21 + .../launch/sensor_manager.launch.xml | 17 + src/wroboarm_22/launch/setup_assistant.launch | 15 + ...imple_moveit_controller_manager.launch.xml | 8 + .../launch/trajectory_execution.launch.xml | 24 + src/wroboarm_22/launch/warehouse.launch | 15 + .../launch/warehouse_settings.launch.xml | 16 + ...rm_22_moveit_controller_manager.launch.xml | 10 + ...oboarm_22_moveit_sensor_manager.launch.xml | 3 + src/wroboarm_22/meshes/base_link.STL | Bin 0 -> 11984 bytes src/wroboarm_22/meshes/forarm_link.STL | Bin 0 -> 19984 bytes src/wroboarm_22/meshes/manipulator_link.STL | Bin 0 -> 54284 bytes src/wroboarm_22/meshes/shoulderBase_link.STL | Bin 0 -> 53584 bytes src/wroboarm_22/meshes/upperArm_link.STL | Bin 0 -> 25984 bytes src/wroboarm_22/meshes/wirstBase_link.STL | Bin 0 -> 12484 bytes src/wroboarm_22/meshes/wrist_link.STL | Bin 0 -> 50884 bytes src/wroboarm_22/package.xml | 36 ++ src/wroboarm_22/urdf/wroboarm_22.csv | 8 + src/wroboarm_22/urdf/wroboarm_22.urdf | 281 +++++++++ test_arm_control_stack.sh | 11 +- 91 files changed, 4107 insertions(+), 260 deletions(-) create mode 100644 src/arm_assembly/CMakeLists.txt create mode 100644 src/arm_assembly/config/joint_names_arm_assembly.yaml create mode 100644 src/arm_assembly/export.log create mode 100644 src/arm_assembly/launch/display.launch create mode 100644 src/arm_assembly/launch/gazebo.launch create mode 100644 src/arm_assembly/meshes/base_link.STL create mode 100644 src/arm_assembly/meshes/forarm_link.STL create mode 100644 src/arm_assembly/meshes/manipulator_link.STL create mode 100644 src/arm_assembly/meshes/shoulderBase_link.STL create mode 100644 src/arm_assembly/meshes/upperArm_link.STL create mode 100644 src/arm_assembly/meshes/wirstBase_link.STL create mode 100644 src/arm_assembly/meshes/wrist_link.STL create mode 100644 src/arm_assembly/package.xml create mode 100644 src/arm_assembly/urdf/arm_assembly.csv create mode 100644 src/arm_assembly/urdf/arm_assembly.urdf create mode 100644 src/wr_control_drive_arm/config/encoder_parameters.yaml create mode 100644 src/wr_control_drive_arm/src/AbstractJoint.cpp create mode 100644 src/wr_control_drive_arm/src/AbstractJoint.hpp create mode 100644 src/wr_control_drive_arm/src/DifferentialJoint.cpp create mode 100644 src/wr_control_drive_arm/src/DifferentialJoint.hpp delete mode 100644 src/wr_control_drive_arm/src/SimpleACS.cpp create mode 100644 src/wr_control_drive_arm/src/SimpleJoint.cpp create mode 100644 src/wr_control_drive_arm/src/SimpleJoint.hpp create mode 100644 src/wr_entry_point/launch/mock_arm.launch create mode 100644 src/wr_entry_point/launch/test/vis_arm.launch create mode 100644 src/wroboarm_22/.setup_assistant create mode 100644 src/wroboarm_22/CMakeLists.txt create mode 100644 src/wroboarm_22/config/arm_assembly.srdf create mode 100644 src/wroboarm_22/config/cartesian_limits.yaml create mode 100644 src/wroboarm_22/config/chomp_planning.yaml create mode 100644 src/wroboarm_22/config/fake_controllers.yaml create mode 100644 src/wroboarm_22/config/gazebo_controllers.yaml create mode 100644 src/wroboarm_22/config/joint_limits.yaml create mode 100644 src/wroboarm_22/config/joint_names_wroboarm_22.yaml create mode 100644 src/wroboarm_22/config/kinematics.yaml create mode 100644 src/wroboarm_22/config/ompl_planning.yaml create mode 100644 src/wroboarm_22/config/ros_controllers.yaml create mode 100644 src/wroboarm_22/config/sensors_3d.yaml create mode 100644 src/wroboarm_22/config/simple_moveit_controllers.yaml create mode 100644 src/wroboarm_22/config/wroboarm_22.srdf create mode 100644 src/wroboarm_22/export.log create mode 100644 src/wroboarm_22/launch/arm_assembly_moveit_sensor_manager.launch.xml create mode 100644 src/wroboarm_22/launch/chomp_planning_pipeline.launch.xml create mode 100644 src/wroboarm_22/launch/default_warehouse_db.launch create mode 100644 src/wroboarm_22/launch/demo.launch create mode 100644 src/wroboarm_22/launch/demo_gazebo.launch create mode 100644 src/wroboarm_22/launch/demo_test.launch create mode 100644 src/wroboarm_22/launch/display.launch create mode 100644 src/wroboarm_22/launch/fake_moveit_controller_manager.launch.xml create mode 100644 src/wroboarm_22/launch/gazebo copy.launch create mode 100644 src/wroboarm_22/launch/gazebo.launch create mode 100644 src/wroboarm_22/launch/joystick_control.launch create mode 100644 src/wroboarm_22/launch/move_group.launch create mode 100644 src/wroboarm_22/launch/moveit.rviz create mode 100644 src/wroboarm_22/launch/moveit_rviz.launch create mode 100644 src/wroboarm_22/launch/ompl_planning_pipeline.launch.xml create mode 100644 src/wroboarm_22/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100644 src/wroboarm_22/launch/planning_context.launch create mode 100644 src/wroboarm_22/launch/planning_pipeline.launch.xml create mode 100644 src/wroboarm_22/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 src/wroboarm_22/launch/ros_controllers.launch create mode 100644 src/wroboarm_22/launch/run_benchmark_ompl.launch create mode 100644 src/wroboarm_22/launch/sensor_manager.launch.xml create mode 100644 src/wroboarm_22/launch/setup_assistant.launch create mode 100644 src/wroboarm_22/launch/simple_moveit_controller_manager.launch.xml create mode 100644 src/wroboarm_22/launch/trajectory_execution.launch.xml create mode 100644 src/wroboarm_22/launch/warehouse.launch create mode 100644 src/wroboarm_22/launch/warehouse_settings.launch.xml create mode 100644 src/wroboarm_22/launch/wroboarm_22_moveit_controller_manager.launch.xml create mode 100644 src/wroboarm_22/launch/wroboarm_22_moveit_sensor_manager.launch.xml create mode 100644 src/wroboarm_22/meshes/base_link.STL create mode 100644 src/wroboarm_22/meshes/forarm_link.STL create mode 100644 src/wroboarm_22/meshes/manipulator_link.STL create mode 100644 src/wroboarm_22/meshes/shoulderBase_link.STL create mode 100644 src/wroboarm_22/meshes/upperArm_link.STL create mode 100644 src/wroboarm_22/meshes/wirstBase_link.STL create mode 100644 src/wroboarm_22/meshes/wrist_link.STL create mode 100644 src/wroboarm_22/package.xml create mode 100644 src/wroboarm_22/urdf/wroboarm_22.csv create mode 100644 src/wroboarm_22/urdf/wroboarm_22.urdf diff --git a/.gitignore b/.gitignore index c41a5ca4..6629522d 100644 --- a/.gitignore +++ b/.gitignore @@ -17,17 +17,15 @@ src/sick_scan src/wreadinput src/wready src/wroboclaw +src/wrosout src/wresponsecontrol # external lib destination dirs src/drive/lib src/manager/lib src/science/lib - -*.pyc src/arm/lib -compile_commands.json -src/wrosout + *.pyc src/arm/lib @@ -35,4 +33,3 @@ src/arm/lib # potential clang excludes compile_commands.json .cache ->>>>>>> origin diff --git a/src/arm_assembly/CMakeLists.txt b/src/arm_assembly/CMakeLists.txt new file mode 100644 index 00000000..22bc5ed7 --- /dev/null +++ b/src/arm_assembly/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(arm_assembly) + +find_package(catkin REQUIRED) + +catkin_package() + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/src/arm_assembly/config/joint_names_arm_assembly.yaml b/src/arm_assembly/config/joint_names_arm_assembly.yaml new file mode 100644 index 00000000..159fc6fe --- /dev/null +++ b/src/arm_assembly/config/joint_names_arm_assembly.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'turntable', 'shoulder', 'elbow', 'forearm_roll', 'wrist_pitch', 'wrist_roll', ] diff --git a/src/arm_assembly/export.log b/src/arm_assembly/export.log new file mode 100644 index 00000000..0f28f844 --- /dev/null +++ b/src/arm_assembly/export.log @@ -0,0 +1,566 @@ +2022-01-28 23:47:18,488 INFO Logger.cs: 70 - +-------------------------------------------------------------------------------- +2022-01-28 23:47:18,508 INFO Logger.cs: 71 - Logging commencing for SW2URDF exporter +2022-01-28 23:47:18,508 INFO Logger.cs: 73 - Commit version 1.6.0-4-g7f85cfe +2022-01-28 23:47:18,509 INFO Logger.cs: 74 - Build version 1.6.7995.38578 +2022-01-28 23:47:18,510 INFO SwAddin.cs: 192 - Attempting to connect to SW +2022-01-28 23:47:18,511 INFO SwAddin.cs: 197 - Setting up callbacks +2022-01-28 23:47:18,511 INFO SwAddin.cs: 201 - Setting up command manager +2022-01-28 23:47:18,512 INFO SwAddin.cs: 204 - Adding command manager +2022-01-28 23:47:18,517 INFO SwAddin.cs: 263 - Adding Assembly export to file menu +2022-01-28 23:47:18,517 INFO SwAddin.cs: 272 - Adding Part export to file menu +2022-01-28 23:47:18,518 INFO SwAddin.cs: 210 - Adding event handlers +2022-01-28 23:47:18,520 INFO SwAddin.cs: 217 - Connecting plugin to SolidWorks +2022-01-29 04:01:04,989 INFO SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM +2022-01-29 04:01:07,687 INFO SwAddin.cs: 313 - Saving assembly +2022-01-29 04:01:07,864 INFO SwAddin.cs: 316 - Opening property manager +2022-01-29 04:01:07,891 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:01:07,894 INFO ExportHelperExtension.cs: 1136 - Found 44 in ArmAssembly.SLDASM +2022-01-29 04:01:07,894 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:01:07,895 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:07,895 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:07,930 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:01:07,930 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:07,931 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:01:07,931 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:01:07,931 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:07,932 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:01:07,932 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:01:07,932 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:07,933 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:01:07,933 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:01:07,934 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:07,934 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:01:07,934 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:07,936 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:01:07,936 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:01:07,936 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:07,937 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:01:07,937 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1136 - Found 44 in ArmAssembly.SLDASM +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:07,939 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:01:07,939 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:07,939 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:01:07,940 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:01:07,940 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:07,940 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:01:07,942 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:07,942 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:01:07,942 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:01:07,943 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:07,943 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:01:07,943 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:01:07,944 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:07,944 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:01:07,944 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:01:07,945 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:07,945 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:01:08,306 INFO SwAddin.cs: 339 - Loading config tree +2022-01-29 04:01:08,309 INFO ExportPropertyManagerExtension.cs: 520 - Starting new configuration +2022-01-29 04:01:08,315 INFO SwAddin.cs: 344 - Showing property manager +2022-01-29 04:01:36,796 INFO ExportPropertyManager.cs: 422 - Configuration saved +2022-01-29 04:01:38,889 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:01:38,895 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:38,895 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:01:38,895 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:38,896 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:38,896 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:01:38,896 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:38,897 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:01:38,897 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:01:38,897 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:38,898 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:01:38,898 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:01:38,899 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:38,899 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:01:38,899 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:01:38,900 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:38,900 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:01:38,900 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:01:38,901 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:38,901 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:01:38,902 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:01:38,902 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:38,902 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:01:38,903 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:01:38,903 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:38,903 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:01:38,904 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:01:38,904 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:38,904 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:01:38,905 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:38,905 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:38,905 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:01:38,906 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:38,906 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:01:38,906 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:38,908 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:01:38,908 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:01:38,908 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:38,909 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:01:38,909 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:01:38,909 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:38,910 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:01:38,910 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:01:38,910 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:38,911 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:01:38,911 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:01:38,911 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:38,912 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:01:38,958 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:01:48,808 INFO SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM +2022-01-29 04:01:48,808 INFO SwAddin.cs: 299 - Save is required +2022-01-29 04:01:48,809 INFO SwAddin.cs: 313 - Saving assembly +2022-01-29 04:01:49,233 INFO SwAddin.cs: 316 - Opening property manager +2022-01-29 04:01:49,234 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:01:49,235 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:49,235 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:01:49,235 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:49,236 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:49,236 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:01:49,236 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:49,237 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:01:49,237 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:01:49,237 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:49,238 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:01:49,238 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:01:49,238 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:49,239 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:01:49,239 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:01:49,240 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:49,240 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:01:49,240 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:01:49,241 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:49,241 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:01:49,241 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:01:49,242 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:49,248 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:01:49,248 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:01:49,279 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:49,279 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:49,281 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:49,281 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:01:49,282 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:49,282 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:01:49,282 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:01:49,283 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:49,283 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:01:49,283 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:01:49,284 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:49,284 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:01:49,285 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:01:49,285 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:49,285 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:01:49,286 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:01:49,286 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:01:49,288 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:01:49,288 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:49,288 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:01:49,398 INFO SwAddin.cs: 339 - Loading config tree +2022-01-29 04:01:49,400 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:01:49,445 INFO LinkNode.cs: 35 - Building node base_link +2022-01-29 04:01:49,446 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM +2022-01-29 04:01:49,447 INFO CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly +2022-01-29 04:01:49,448 INFO CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\Tests\URDF_Assembly\base.SLDPRT +2022-01-29 04:01:49,449 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link base_link +2022-01-29 04:01:49,471 INFO SwAddin.cs: 344 - Showing property manager +2022-01-29 04:02:15,278 INFO ExportPropertyManager.cs: 422 - Configuration saved +2022-01-29 04:02:15,279 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:02:15,425 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:02:15,426 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:02:15,426 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:02:15,427 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:02:15,427 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:02:15,427 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:02:15,428 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:02:15,428 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:02:15,428 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:02:15,429 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:02:15,429 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:02:15,429 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:02:15,431 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:02:15,431 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:02:15,431 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:02:15,432 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:02:15,432 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:02:15,432 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:02:15,433 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:02:15,433 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:02:15,433 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:02:15,434 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:02:15,434 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:02:15,434 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:02:15,435 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:02:15,435 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:02:15,435 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:02:15,436 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:02:15,436 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:02:15,436 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:02:15,437 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:02:15,437 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:02:15,437 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:02:15,438 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:02:15,438 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:02:15,438 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:02:15,439 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:02:15,439 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:02:15,439 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:02:15,440 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:02:15,440 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:02:15,442 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:02:15,442 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:02:15,443 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:02:15,488 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:03:44,437 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:03:50,608 INFO SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM +2022-01-29 04:03:50,608 INFO SwAddin.cs: 299 - Save is required +2022-01-29 04:03:50,608 INFO SwAddin.cs: 313 - Saving assembly +2022-01-29 04:03:50,733 INFO SwAddin.cs: 316 - Opening property manager +2022-01-29 04:03:50,733 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:03:50,734 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:03:50,734 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:03:50,734 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:03:50,735 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:03:50,735 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:03:50,736 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:03:50,736 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:03:50,736 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:03:50,737 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:03:50,737 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:03:50,738 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:03:50,738 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:03:50,739 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:03:50,739 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:03:50,740 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:03:50,740 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:03:50,740 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:03:50,741 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:03:50,741 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:03:50,741 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:03:50,742 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:03:50,743 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:03:50,743 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:03:50,743 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:03:50,744 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:03:50,744 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:03:50,745 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:03:50,745 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:03:50,745 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:03:50,746 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:03:50,746 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:03:50,746 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:03:50,747 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:03:50,747 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:03:50,747 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:03:50,748 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:03:50,748 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:03:50,748 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:03:50,749 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:03:50,749 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:03:50,749 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:03:50,750 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:03:50,750 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:03:50,750 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:03:50,751 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:03:50,751 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:03:50,752 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:03:50,752 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:03:50,752 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:03:50,753 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:03:50,753 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:03:50,802 INFO SwAddin.cs: 339 - Loading config tree +2022-01-29 04:03:50,803 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue9.1830074983591922E-190.0156064900891439551.5715252581589567E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504422ixytrue8.02209898558041E-21ixztrue-7.374852413660824E-20iyytrue0.0044229329573873041iyztrue2.2568808327940613E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:03:50,803 INFO LinkNode.cs: 35 - Building node base_link +2022-01-29 04:03:50,804 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM +2022-01-29 04:03:50,804 INFO CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly +2022-01-29 04:03:50,804 INFO CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\Tests\URDF_Assembly\base.SLDPRT +2022-01-29 04:03:50,805 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link base_link +2022-01-29 04:03:50,813 INFO SwAddin.cs: 344 - Showing property manager +2022-01-29 04:03:55,199 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:04:02,915 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:04:06,718 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:04:08,432 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:07:25,776 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:07:36,624 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:08:34,715 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:09:06,042 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:09:28,401 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:09:33,716 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:10:26,057 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:10:32,832 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:01,760 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:10,262 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:32,320 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:50,721 INFO ExportPropertyManager.cs: 422 - Configuration saved +2022-01-29 04:11:50,723 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue9.1830074983591922E-190.0156064900891439551.5715252581589567E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504422ixytrue8.02209898558041E-21ixztrue-7.374852413660824E-20iyytrue0.0044229329573873041iyztrue2.2568808327940613E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:11:50,809 INFO ExportHelperExtension.cs: 347 - Creating joint shoulderBase_link +2022-01-29 04:11:50,815 INFO ExportHelperExtension.cs: 1397 - Fixing components for base_link +2022-01-29 04:11:50,816 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:50,816 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:51,179 INFO : 0 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:51,183 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:51,183 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:51,184 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:53,156 WARN ExportHelperExtension.cs: 351 - Creating joint from parent base_link to child shoulderBase_link failed +2022-01-29 04:11:53,162 INFO ExportHelperExtension.cs: 347 - Creating joint upperArm_link +2022-01-29 04:11:53,163 INFO ExportHelperExtension.cs: 1397 - Fixing components for shoulderBase_link +2022-01-29 04:11:53,163 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:53,163 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:53,164 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:53,382 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:53,383 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:53,383 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:53,383 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:53,384 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:11:55,357 WARN ExportHelperExtension.cs: 351 - Creating joint from parent shoulderBase_link to child upperArm_link failed +2022-01-29 04:11:55,362 INFO ExportHelperExtension.cs: 347 - Creating joint forarm_link +2022-01-29 04:11:55,362 INFO ExportHelperExtension.cs: 1397 - Fixing components for upperArm_link +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:55,607 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:55,607 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:55,607 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:55,608 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:55,608 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:11:55,608 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:11:57,637 WARN ExportHelperExtension.cs: 351 - Creating joint from parent upperArm_link to child forarm_link failed +2022-01-29 04:11:57,644 INFO ExportHelperExtension.cs: 347 - Creating joint wirstBase_link +2022-01-29 04:11:57,644 INFO ExportHelperExtension.cs: 1397 - Fixing components for forarm_link +2022-01-29 04:11:57,645 INFO ExportHelperExtension.cs: 1402 - Fixing 35 +2022-01-29 04:11:57,645 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:11:57,645 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:57,646 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:57,646 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:57,884 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:57,885 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:57,885 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:57,886 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:57,886 INFO ExportHelperExtension.cs: 1352 - Unfixing component 35 +2022-01-29 04:11:57,886 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:11:57,887 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:11:59,971 WARN ExportHelperExtension.cs: 351 - Creating joint from parent forarm_link to child wirstBase_link failed +2022-01-29 04:11:59,975 INFO ExportHelperExtension.cs: 347 - Creating joint wrist_link +2022-01-29 04:11:59,975 INFO ExportHelperExtension.cs: 1397 - Fixing components for wirstBase_link +2022-01-29 04:11:59,976 INFO ExportHelperExtension.cs: 1402 - Fixing 40 +2022-01-29 04:11:59,976 INFO ExportHelperExtension.cs: 1402 - Fixing 35 +2022-01-29 04:11:59,976 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:11:59,977 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:59,977 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:59,977 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:12:00,240 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:12:00,241 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:12:00,241 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:12:00,241 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:12:00,242 INFO ExportHelperExtension.cs: 1352 - Unfixing component 40 +2022-01-29 04:12:00,242 INFO ExportHelperExtension.cs: 1352 - Unfixing component 35 +2022-01-29 04:12:00,242 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:12:00,243 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:12:02,368 WARN ExportHelperExtension.cs: 351 - Creating joint from parent wirstBase_link to child wrist_link failed +2022-01-29 04:12:02,372 INFO ExportHelperExtension.cs: 347 - Creating joint manipulator_link +2022-01-29 04:12:02,373 INFO ExportHelperExtension.cs: 1397 - Fixing components for wrist_link +2022-01-29 04:12:02,373 INFO ExportHelperExtension.cs: 1402 - Fixing 39 +2022-01-29 04:12:02,373 INFO ExportHelperExtension.cs: 1402 - Fixing 40 +2022-01-29 04:12:02,374 INFO ExportHelperExtension.cs: 1402 - Fixing 35 +2022-01-29 04:12:02,374 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:12:02,374 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:12:02,375 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:12:02,375 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:12:02,655 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:12:02,655 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:12:02,656 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:12:02,656 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:12:02,698 INFO ExportHelperExtension.cs: 1352 - Unfixing component 39 +2022-01-29 04:12:02,699 INFO ExportHelperExtension.cs: 1352 - Unfixing component 40 +2022-01-29 04:12:02,699 INFO ExportHelperExtension.cs: 1352 - Unfixing component 35 +2022-01-29 04:12:02,699 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:12:02,700 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:12:04,927 WARN ExportHelperExtension.cs: 351 - Creating joint from parent wrist_link to child manipulator_link failed +2022-01-29 04:12:05,029 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:12:05,030 INFO ExportHelperExtension.cs: 1136 - Found 59 in ArmAssembly.SLDASM +2022-01-29 04:12:05,031 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:12:05,031 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:12:05,031 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:12:05,032 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:12:05,032 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:12:05,032 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:12:05,033 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:12:05,033 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:12:05,033 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:12:05,034 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:12:05,034 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:12:05,035 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:12:05,035 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:12:05,035 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:12:05,036 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:12:05,036 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:12:05,036 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:12:05,037 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:12:05,037 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:12:05,037 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:12:05,038 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:12:05,038 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:12:05,038 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:12:05,039 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:12:05,039 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:12:05,039 INFO ExportHelperExtension.cs: 1136 - Found 59 in ArmAssembly.SLDASM +2022-01-29 04:12:05,040 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:12:05,040 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:12:05,040 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:12:05,041 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:12:05,041 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:12:05,041 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:12:05,042 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:12:05,042 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:12:05,042 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:12:05,044 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:12:05,044 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:12:05,044 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:12:05,045 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:12:05,045 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:12:05,045 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:12:05,046 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:12:05,046 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:12:05,046 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:12:05,047 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:12:05,047 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:12:05,103 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:13:17,527 INFO AssemblyExportForm.cs: 253 - Completing URDF export +2022-01-29 04:13:17,529 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue9.1830074983591922E-190.0156064900891439551.5715252581589567E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504422ixytrue8.02209898558041E-21ixztrue-7.374852413660824E-20iyytrue0.0044229329573873041iyztrue2.2568808327940613E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruenametrueshoulderBase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueturntabletypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically GeneratelinktruenametrueupperArm_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueshouldertypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueforarm_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueelbowtypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically GeneratelinktruenametruewirstBase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueforearm_rolltypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametruewrist_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_pitchtypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametruemanipulator_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_rolltypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==falsefalsefalsesDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==falsefalsefalsesDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==falsefalsefalsesDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAAfalsefalsefalsesDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==falsefalsefalsesDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==falsefalsefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:13:41,728 INFO AssemblyExportForm.cs: 309 - Saving URDF package to E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM +2022-01-29 04:13:41,730 INFO ExportHelper.cs: 147 - Beginning the export process +2022-01-29 04:13:41,731 INFO ExportHelper.cs: 153 - Creating package directories with name ArmAssembly.SLDASM and save path E:\Robotics\WR\Tests\URDF_Assembly +2022-01-29 04:13:46,142 ERROR AssemblyExportForm.cs: 126 - Exception encountered in Assembly export form +System.IO.IOException: Cannot create "E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM" because a file or directory with the same name already exists. + at System.IO.__Error.WinIOError(Int32 errorCode, String maybeFullPath) + at System.IO.Directory.InternalCreateDirectory(String fullPath, String path, Object dirSecurityObj, Boolean checkHost) + at System.IO.Directory.InternalCreateDirectoryHelper(String path, Boolean checkHost) + at SW2URDF.URDFExport.URDFPackage.CreateDirectories() in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\URDFExport\URDFPackage.cs:line 78 + at SW2URDF.URDFExport.ExportHelper.ExportRobot(Boolean exportSTL) in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\URDFExport\ExportHelper.cs:line 155 + at SW2URDF.UI.AssemblyExportForm.FinishExport(Boolean exportSTL) in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\UI\AssemblyExportForm.cs:line 310 + at SW2URDF.UI.AssemblyExportForm.ButtonLinksFinishClick(Object sender, EventArgs e) in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\UI\AssemblyExportForm.cs:line 243 + at System.Windows.Forms.Control.OnClick(EventArgs e) + at System.Windows.Forms.Button.OnClick(EventArgs e) + at System.Windows.Forms.Button.OnMouseUp(MouseEventArgs mevent) + at System.Windows.Forms.Control.WmMouseUp(Message& m, MouseButtons button, Int32 clicks) + at System.Windows.Forms.Control.WndProc(Message& m) + at System.Windows.Forms.ButtonBase.WndProc(Message& m) + at System.Windows.Forms.Button.WndProc(Message& m) + at System.Windows.Forms.NativeWindow.Callback(IntPtr hWnd, Int32 msg, IntPtr wparam, IntPtr lparam) +2022-01-29 04:13:53,196 INFO AssemblyExportForm.cs: 253 - Completing URDF export +2022-01-29 04:13:53,197 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue1.035828411313638E-180.0156064900891439571.9357498027169635E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504418ixytrue8.6586155170563642E-21ixztrue-7.3748524136608228E-20iyytrue0.0044229329573873041iyztrue2.2771129801693437E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametrueshoulderBase_linkxyztrue-0.0122586821821900790.1285772495779757-0.0037054435004292684rpytrue000originfalsefalsevaluetrue0.99636680500272734massfalseixxtrue0.0065237009420894719ixytrue0.0010646616036528998ixztrue0.00028312700527736404iyytrue0.0050231810993567032iyztrue6.82113719161566E-05izztrue0.0086334538259584inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueturntabletypetruecontinuousxyztrue000.035865rpytrue1.57080-0.25129originfalsefalselinktruebase_linkparenttruelinktrueshoulderBase_linkchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_turntableOrigin_turntablelinktruenametrueupperArm_linkxyztrue0.014397399905456463-3.2006162568792851E-050.27934838479492807rpytrue000originfalsefalsevaluetrue0.98521034391581119massfalseixxtrue0.023798222473833903ixytrue-8.87977733299251E-07ixztrue-0.0038853285623011904iyytrue0.029359472259760169iyztrue-7.1294436997127461E-06izztrue0.0062503483932010362inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueshouldertypetruecontinuousxyztrue00.180980.043071rpytrue-1.102100originfalsefalselinktrueshoulderBase_linkparenttruelinktrueupperArm_linkchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_shoulderOrigin_shoulderlinktruenametrueforarm_linkxyztrue1.8457457784393227E-150.0550154986611603450.12015280303424725rpytrue000originfalsefalsevaluetrue0.51779610173045887massfalseixxtrue0.00404850874710771ixytrue2.1141942363467336E-18ixztrue-1.5487150407597627E-17iyytrue0.003755483293922739iyztrue-0.00037738536370652919izztrue0.00078832783822656164inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueelbowtypetruecontinuousxyztrue000.46634rpytrue2.493600originfalsefalselinktrueupperArm_linkparenttruelinktrueforarm_linkchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_elbowOrigin_elbowlinktruenametruewirstBase_linkxyztrue-1.9428902930940239E-16-1.6653345369377348E-150.09870348455642336rpytrue000originfalsefalsevaluetrue0.30176308217533721massfalseixxtrue0.0021034428970283731ixytrue-2.7105054312137611E-19ixztrue-2.2497195079074217E-18iyytrue0.003050162507065583iyztrue1.8566962203814263E-18izztrue0.0010760910650178595inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueforearm_rolltypetruecontinuousxyztrue00.044450.2285rpytrue000originfalsefalselinktrueforarm_linkparenttruelinktruewirstBase_linkchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_forearm_rollOrigin_forearm_rolllinktruenametruewrist_linkxyztrue4.3021142204224816E-16-2.7755575615628914E-170rpytrue000originfalsefalsevaluetrue0.44770711794317625massfalseixxtrue0.00036770226690479489ixytrue1.7999450129153882E-19ixztrue-7.453889935837843E-19iyytrue0.00084342708626575322iyztrue-1.3552527156068805E-20izztrue0.00084474612356065992inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_pitchtypetruecontinuousxyztrue000.23716rpytrue-1.33300originfalsefalselinktruewirstBase_linkparenttruelinktruewrist_linkchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_wrist_pitchOrigin_wrist_pitchlinktruenametruemanipulator_linkxyztrue-0.00073032374046050341-0.000899365669252000230.049056009995342265rpytrue000originfalsefalsevaluetrue0.39742022555481077massfalseixxtrue0.0015968430526387824ixytrue-3.0724322789165625E-06ixztrue2.5323557993615128E-05iyytrue0.0016343224414687998iyztrue5.0029229087463509E-05izztrue0.00025632256399428705inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_rolltypetruecontinuousxyztrue000rpytrue000.096862originfalsefalselinktruewrist_linkparenttruelinktruemanipulator_linkchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_wrist_rollOrigin_wrist_rolllinktruefalsesDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==falsefalsefalsesDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==falsefalsefalsesDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==falsefalsefalsesDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAAfalsefalsefalsesDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==falsefalsefalsesDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==falsefalsefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:14:02,877 INFO AssemblyExportForm.cs: 309 - Saving URDF package to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM +2022-01-29 04:14:02,877 INFO ExportHelper.cs: 147 - Beginning the export process +2022-01-29 04:14:02,877 INFO ExportHelper.cs: 153 - Creating package directories with name ArmAssembly.SLDASM and save path E:\Robotics\WR\Tests\URDF_Assembly\URDF +2022-01-29 04:14:04,383 INFO ExportHelper.cs: 162 - Creating CMakeLists.txt at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\CMakeLists.txt +2022-01-29 04:14:04,384 INFO ExportHelper.cs: 166 - Creating joint names config at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\config\joint_names_ArmAssembly.SLDASM.yaml +2022-01-29 04:14:04,385 INFO ExportHelper.cs: 170 - Creating package.xml at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\package.xml +2022-01-29 04:14:04,385 INFO PackageXMLWriter.cs: 21 - Creating package.xml at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\package.xml +2022-01-29 04:14:04,389 INFO ExportHelper.cs: 177 - Creating RVIZ launch file in E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\launch\ +2022-01-29 04:14:04,391 INFO ExportHelper.cs: 182 - Creating Gazebo launch file in E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\launch\ +2022-01-29 04:14:04,392 INFO ExportHelper.cs: 187 - Saving existing STL preferences +2022-01-29 04:14:04,392 INFO ExportHelper.cs: 450 - Saving users preferences +2022-01-29 04:14:04,393 INFO ExportHelper.cs: 190 - Modifying STL preferences +2022-01-29 04:14:04,394 INFO ExportHelper.cs: 464 - Setting STL preferences +2022-01-29 04:14:04,397 INFO ExportHelper.cs: 196 - Found 0 hidden components +2022-01-29 04:14:04,398 INFO ExportHelper.cs: 197 - Hiding all components +2022-01-29 04:14:04,618 INFO ExportHelper.cs: 204 - Beginning individual files export +2022-01-29 04:14:04,622 INFO ExportHelper.cs: 270 - Exporting link: base_link +2022-01-29 04:14:04,623 INFO ExportHelper.cs: 272 - Link base_link has 1 children +2022-01-29 04:14:04,624 INFO ExportHelper.cs: 270 - Exporting link: shoulderBase_link +2022-01-29 04:14:04,624 INFO ExportHelper.cs: 272 - Link shoulderBase_link has 1 children +2022-01-29 04:14:04,625 INFO ExportHelper.cs: 270 - Exporting link: upperArm_link +2022-01-29 04:14:04,625 INFO ExportHelper.cs: 272 - Link upperArm_link has 1 children +2022-01-29 04:14:04,625 INFO ExportHelper.cs: 270 - Exporting link: forarm_link +2022-01-29 04:14:04,626 INFO ExportHelper.cs: 272 - Link forarm_link has 1 children +2022-01-29 04:14:04,626 INFO ExportHelper.cs: 270 - Exporting link: wirstBase_link +2022-01-29 04:14:04,627 INFO ExportHelper.cs: 272 - Link wirstBase_link has 1 children +2022-01-29 04:14:04,628 INFO ExportHelper.cs: 270 - Exporting link: wrist_link +2022-01-29 04:14:04,628 INFO ExportHelper.cs: 272 - Link wrist_link has 1 children +2022-01-29 04:14:04,629 INFO ExportHelper.cs: 270 - Exporting link: manipulator_link +2022-01-29 04:14:04,629 INFO ExportHelper.cs: 272 - Link manipulator_link has 0 children +2022-01-29 04:14:04,630 INFO ExportHelper.cs: 317 - manipulator_link: Exporting STL with coordinate frame Origin_wrist_roll +2022-01-29 04:14:04,631 INFO ExportHelper.cs: 322 - manipulator_link: Reference geometry name +2022-01-29 04:14:04,669 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\manipulator_link.STL +2022-01-29 04:14:04,785 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,785 INFO ExportHelper.cs: 317 - wrist_link: Exporting STL with coordinate frame Origin_wrist_pitch +2022-01-29 04:14:04,786 INFO ExportHelper.cs: 322 - wrist_link: Reference geometry name +2022-01-29 04:14:04,797 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\wrist_link.STL +2022-01-29 04:14:04,834 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,835 INFO ExportHelper.cs: 317 - wirstBase_link: Exporting STL with coordinate frame Origin_forearm_roll +2022-01-29 04:14:04,835 INFO ExportHelper.cs: 322 - wirstBase_link: Reference geometry name +2022-01-29 04:14:04,846 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\wirstBase_link.STL +2022-01-29 04:14:04,873 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,874 INFO ExportHelper.cs: 317 - forarm_link: Exporting STL with coordinate frame Origin_elbow +2022-01-29 04:14:04,874 INFO ExportHelper.cs: 322 - forarm_link: Reference geometry name +2022-01-29 04:14:04,932 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\forarm_link.STL +2022-01-29 04:14:04,986 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,987 INFO ExportHelper.cs: 317 - upperArm_link: Exporting STL with coordinate frame Origin_shoulder +2022-01-29 04:14:04,987 INFO ExportHelper.cs: 322 - upperArm_link: Reference geometry name +2022-01-29 04:14:05,001 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\upperArm_link.STL +2022-01-29 04:14:05,035 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:05,035 INFO ExportHelper.cs: 317 - shoulderBase_link: Exporting STL with coordinate frame Origin_turntable +2022-01-29 04:14:05,036 INFO ExportHelper.cs: 322 - shoulderBase_link: Reference geometry name +2022-01-29 04:14:05,111 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\shoulderBase_link.STL +2022-01-29 04:14:05,189 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:05,190 INFO ExportHelper.cs: 317 - base_link: Exporting STL with coordinate frame Origin_global +2022-01-29 04:14:05,191 INFO ExportHelper.cs: 322 - base_link: Reference geometry name +2022-01-29 04:14:05,235 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\base_link.STL +2022-01-29 04:14:05,274 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:05,275 INFO ExportHelper.cs: 145 - Showing all components except previously hidden components +2022-01-29 04:14:05,465 INFO ExportHelper.cs: 145 - Resetting STL preferences +2022-01-29 04:14:05,466 INFO ExportHelper.cs: 478 - Returning STL preferences to user preferences +2022-01-29 04:14:05,466 INFO ExportHelper.cs: 228 - Writing URDF file to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\urdf\ArmAssembly.SLDASM.urdf +2022-01-29 04:14:05,470 INFO CSVImportExport.cs: 32 - Writing CSV file E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\urdf\ArmAssembly.SLDASM.csv +2022-01-29 04:14:05,493 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,494 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,495 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,495 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,495 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,496 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,496 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,497 INFO ExportHelper.cs: 234 - Copying log file +2022-01-29 04:14:05,497 INFO ExportHelper.cs: 439 - Copying C:\Users\Payton Jackson\sw2urdf_logs\sw2urdf.log to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\export.log diff --git a/src/arm_assembly/launch/display.launch b/src/arm_assembly/launch/display.launch new file mode 100644 index 00000000..96a2d12e --- /dev/null +++ b/src/arm_assembly/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + \ No newline at end of file diff --git a/src/arm_assembly/launch/gazebo.launch b/src/arm_assembly/launch/gazebo.launch new file mode 100644 index 00000000..57481cd1 --- /dev/null +++ b/src/arm_assembly/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + \ No newline at end of file diff --git a/src/arm_assembly/meshes/base_link.STL b/src/arm_assembly/meshes/base_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..c298f6b589bed4d5547bebeb7d6d33a87c54cc6a GIT binary patch literal 11984 zcmb_hZHyjO8J#R7f~8vpexM12?E6wQYSI8kC_C>662%6M8d6AVqX{IG$|jbExRywz zu24%G(5%LfDzrO@3urV_qs`7cKxlw8SS++gBZd$(TKpgqu_k^!ANS0AX6Ig(AHIKj zy3aY!nYka&y?1Vw{eORdk#xVf^1a2LSKe0aU$?rPKkezf9^akIzZU~PHu2!zSHH3^ z|LE%d`Dq)U+M8A|3s^H{*$F50i)-$2}@6qkK!@avVIEzZ3DOC?8uHSa?!@0r}PH5=?+SS}tZ z1+#$N<1KIfPChlcwdg&zv*ZY{=AGHRqFi&z*xPgu)c@1TQ%wrX{)G$S-|cgJvc<4a|BrXL;AlOjjNP`S-|cgBeBNV z2}@NG+*ROE?0o^IVvf))=iT zR!gD^W&yiLmVLfP6%3I|Bh13z#bZLbU4=#>iuP00QWN{gCI7;sQgwKzO5RC4!kc## z%mUU-R^hZiYN?5RjsR=k&^MLzjd+Cq=qQ*4>>gp1)KU}s90At6VT@HW#(Gv0btHBa z%mQ|gFxqRWiG7X$Yu+$dDw!+dky0=V*ge9mt)(XRIRdPC!<-+=oEMLjf?2@s5voru zHL=eTV9h(T`Ocoy#G%wK@kl9{1>AUO-8B!60Bc_H)7l>1Bh=lFf?2>#Tj}~?ZN(8_ zeGh3jU86#Og!QANU>2}@NDt~dXnoERVC@g-e_dXvdX{_V4df})P0xLm1cj{Q7{YG zJ*2`FR1+8P;ZPs#?_PIf3XO1HV6Ao``>I`iRUW-?Z+`k$pI=b-^X{L`56{1YJq))M z$hBB;^1`k8lh1x~2?evzZknm>!4X!BU3_I;|Mj+r2P>F`{_lFon^$=AnPc-+Z`1p% z6@;m)TEv4RtXMYvKy||px9OU)=sj4$tnl|dsjvtu#-I6ab@K~*Cc6rLLwSkM<(~}D2%f*MlRhxP8ai*aD)}_`qK^7JGV?Pple{?^;5^tT3h{3XZVilneg5 z@Z$QrmryV(%*}{G`XEQINpI#@0i>Urok6Dun+PjpEyRHI;4@Tlb9?y9B=i?Bl0 z#2l+=^r=k2EZ%*z72GD=R-sl7x=+OsR$TDPtqb#aelOCxtY8+tYjiz0!U}zMSq&l{ ztYDUS>)9aw9<2!Sir-}NJsJhGqz(0K5P2$&utM5xGCp=$!7P~tdNzo7aD){ySMvBA zWCgQiHrw~aZhvru6;fmDD`!{15mrc*$dlEl$;B+m-OQWz*yRW-q}t`lD%#{?7SI32 zqtve;s6P5V33EliUt#y*=UUUO+GPrkup*4aa7txGXk9y#g}$*fAgkL?#gRn{I;FA) zD+D*J*Y{Mc4m}%AckICtR$%_?x5f5*u!32b&HBZ&t>8#_cKfEwnc51Dup+cV`0D%` z?_qOW`yBH|$3oj9JiGO6cys$wAl|z73AX@?JemkA!gvec09e5uoR`O2`>8m>3ar}t z{W7g!R(Pts`{tN~d=l2vZ)o-So7+4OUp8%pBSm^#S+0=RSg1<+ z!1iS`v|IeCv1I;hPNiIY(I09wk42@zVUjzOD7f-yHAZ>~#G1 zr{*~0C|~od2xDikCcL0!0xf>x;NI>e*CI> z`_8F8M}RdiPA~3}QZNhHJ;vr&)%U*tqWX&USM)gota)*IagUUOS-|eGV*5wxZ`^Wv zea^vNpCiDU7v~-KNGX^F>>lNF8|pPbdUiPetz$io0Bc^Hque80!jcw)MKdgG?y z8RvYl#}Q!7i?2q(BcWgxuzTp-$bNTeb@e~jPH+TR^Wv+Kd!!W10(Os=ul-#8@BMEc z+%r6Tjw8UD7bi>iNGX^F>>jf(ZK^+g(|v=t|Kz8090At6IDfiFO2I5(_c;3F_4Sxs zAB?S9UUCFj^WyyJ9w`O0fZgNJ^dI17k zSe*ar`jR8SnpbwRcCK}gl!95n?s4DlW5b{AxTfg;YfH%yV9kp&m3yQV%mQ|g_Y`*x z*Y3Hgc2}@NM$Hc zOE?0od8Mw{w;K0IDVPQ99#Y{7)I^Q|YhJ03_6^8AQVM1PyN6Wj0=1nZz?xU;ynQKh zkCcL0!0sU{PJuOwBfy$h)+_r`Bd}6x5TFVh&%`5A!-E6r>O2I7P;PKps z0(%CI0Bc^^KiG{~@JJ|_1?(QOlPR$G;Rvwim3@%itGP!?!7O0+kR4fpJsd}XHLvXF z>|V`1QVM1PyNB#73+zQX0<3vuUuw5Y?vYY33)nqmhdjWZnj^rPSN7L-yW}1z1+#$N zLw5QDoDDbvta;^}V7E)|ky0=V*gbG6(i00ufHg0leeB$oW#PUSk?xaZ+04lk6aG}D z2?}W+yM57!yocTPfE8D$8WC2=6WVpAT|N8XxMevwha3R$V`=BMkSV!Q{lxJ8sBzu>_;B>gk*;gCuHF2BfA>7SvHOm=VZ z+E2wSZV}BSzu>`p5#IYKW$CA41@F#OEx*WnFl%v(j1;`;^DaY^F8x%j;C+y) zW^s$S0194Tc|BLw(oe+--f^@&I2W_HMcN8p$#~`DOl<{6Six(1TEQ%Ck+y=@3|^zy TyRF~|D|i)6E11PCl4bt`R@fV) literal 0 HcmV?d00001 diff --git a/src/arm_assembly/meshes/forarm_link.STL b/src/arm_assembly/meshes/forarm_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..fb511b3442ca5d1468b35914cc8edda55dd1078d GIT binary patch literal 19984 zcmb_kX^dt?6)lM)V-z$mnDB{g;yKl1Vpz~86=N|decJ~_&Z$5GF)w7CF ztK%Pj!0HX(Jue~dJK|OC3C}si^0<6PHAHu#YIdK|UjDG(&WD9h%+Lz8q71|r-gMaN zKYnv&;uu1x73DaBhyHFyd&%N98_VNnR6`jY#~yaX>i*;A62}littdy1t$*LyzU7|p zuZHmBk!mP|W8=0bto~=qA&FxMp;nY5$A7LoyS?M@-&qS``&U&%863C#`=HgmFT8aO zIfNmET2YQ1*FO3c?U#3MYD2hvNi~$=_l=Lad-9ptPmLqunAe@$?znVgd+NV7%&A5Z z=!Ki#@y!&~ZBKe>`-nFj-X3(%eY1*CEA&`<@TCc{>5TdItj$lgs199L4bk1GwqEqa zcFU7rVu+V7(+crY24e2a#mPy#o}M^{5Nbs^Wxn+%Yp*`@ZN_o_vT7)Ug4dh z?45-i!Z8B~wW1t3KKrhRu08l$A2N=smQ_PJ`n`E{#5qfeV+av~TJ3Q}=9rHh?Q#gJ zDG1{mmpqK47ok>^bFJoMt$v6++!r*I(|C-_cv!7^5mw!7MLBZV2sS?E3)f*h+zJ}X z;IQ!@qjzK7h7f8+IdWJhHr|N~yU|zKmv!Zs{mSDJVcp(Hw>ttFMOf$0mGk3Ck9X6h!+?Yov#Lm3=4&(D?hV+f&E zlp}|&IE}ASVe?2el)+)^)g0DZ$RP|N)QWQCu$8m%l{4Hjq6WS&np|IbJR)p$Ze(?K zc4!oV9xLnlsXyD^Y&m61^ZH9Zh8;jgsFm#o#${h})A`$))pL$*LO5!s&k7ON2;0dt zzLN>tu`kKKC<9@;nsK#TYPegX74lFk+eMABhl`opj;x70vUkq(S%ITQ*gkPS?h`M; z9xnT$4210&XR&{T?EA!Cgj!LKQH6yKGa+1OGw4F2V*r_Bpf$AJuplgwyi3*H>lFu# zCtH7Tb&6^Tp;o$QtT|Mpi1Eixo4o&-Ydsu_P%9k&(&8Tm5Y#B*%YQj#^5^qzz8{2I z9r@T}CMTTx%O1qOBYrUZw*!9T8jEC1KCE4ntM2GQs74XPc}yX4w#L0e?z+0FQNIr? z53?FGTaB>hP>mw2ZsWn~=K81=Je6}$`%0~B1jp=*VT5WFVLdkP?_EWxm7b4v=BiNy z&%t=sY94m3lL87QBjjPkdq^r-vdHeM0*g`%2ZUT zR@4ZGBUq6xhaEZEtS#m^9*in^@*JzaB6|ms^5_{s`jV(7kW)o^IH;LM*zaz=ejdh$ zQL7^CA}n4XIdCWfl|{tsBWM-Tu?xEhoCoGBcRQ=gbEO}_?yDnQkAnz{W)?mX70~_N zPL0M6IpyRY>M>UYts+{^58I91xoESG@AQEsjbf(yrMMB;Ew55l8Gt*WRz?!@6cOUUNQ zkK2gao#Q?PG{%t;lrhiWKLS>awwD7rU{%f>x_D8kQeb5vU>%SPjs}lzE&x zfGz?WMKHp2#S;1vYDJDbf}N<+2zusRub5w*x%whU8bQyzmm_NwVR9a)4u>MLm2vy= z-dbgi`dxQCGD5A$!Rj8k{Kt>%TD#{(@ALBM#urVs4umO+~s<(Mwq2{f+8UEI5;*hCJygiYSc=&y-RCEig#8K zka-*&7vLO3ty=Y^RxyuqZBQf9csK|BM!tgBzm&$@ITS$|9Eiq{o=1(KM*TwOar|Gc z27Ye|v!z+-eI~!7*E@@5O&=o7iL!FS3Oj)4>*moXue#LlKq0DfTz5;grXY4c;t#XS z7hlf%S2aiWrB?8SH7eyXg`h?e`=0Xp@gaX&?m?*LiFbTz{Jzt!$$8L|6?cF`5g$MQ zqVd^>+}OjR@v0S$;|_Fx9@HoTd}rMFi~B*S74o~~*}v^Um}WwaAtv-|QF+JoMwK;+ zum~rEnP-Grfup~=Y6T7%j{$`GQUvnrA9F>h6^;+~Zq_KmdTeE|cQc~4DxF{VmHEvh zh2GtE-d#Rw7S#jj5=Gz%#4FFayJyU6U&RWUv)}5dYE3~PCq_7oP%G4p{bq=Ir|-4G zECW7{0iuP>9a(i%lr?>b)Vp#o1HPCc<;16R$rOS;I*s6cc;r#8=wYP@TJd={cB!mV)Jxbt4L&Le4Xhjx{_rNrl>C>+-Hs+mUEnE3v2eFUw$rNFJ%W#*xQd5f-8E5d~3dl@YW;RO~lF zBo1m)=I$4A;`86@%oRbal=(on>zJpy`8khh7-h{t4g4ws72$clF2bz_qDq-tR32~Q z$Oyg5nK*h7v;vyvzgIWjZ@2xTW#%1?wTLoePjcz@hzJFNjES%)ePuw^Aqz)Bn5Flp z`h{#`9tejcs@bRFFw2}r;=?r+>NH)e zQeD@;N)d@W_U?3bGlEu$qdbYLqC&@U1Qo#!?N(P2v;qf4uy;WW%lVoD2GPKX9~M?v#AII26T z%)=41lJRhk8j-47Az~h754Tb82y;AARAnteG|a0;P@{e!^SJ%LaIfAe^0N}Y)GFn% zg1ov2%fmH_fXw6Is_tJQkBp$4#$&~P_gd8mV|I=Dh0NpNsP4mIJTiiE%%kk#YJ~Y) zfgkl7`3j=CpNPK72+ER&>uN-b#3J*4&1fD6e|2A!c{qYrG9K=sMx=^Xh{#v=i3jQ( zVU9PoFp$N6oXzN;`OBgS|BtY!u zDIc#9s!@b~Lotj{D~+~;$W>H?ehV^;sIAHuDqRG-8j)ZJfXwYI@bB)rBd8ihVEl=8 zj(p#T@1f%TZ@%{mnQzc`-wS89(6G9Y+0$KwY7}98EZ=`vm*l7vp;r9rqRYW|Xra-Z z_#S#MB7bYCh~doDms;^%?kxITxXb)4$Hsx^g=o!{k9gj#98)f`&g5cc|e^YE+VR3mClLGZozZd8g; zE9)5lrlE^ajUt9eP!VdyZx6a0I#OH>qE)GMow<&hBJ6zhRiewG2(_}Y@s**AP>mvN z1bsE=B5cg_7d%|sVWm~C^H7Z<_-=idLlJ6a$NgKGE<$G+euD)o?a(?Z)hL2rS9LiQ zp;ny1y9m`NLTgw@r3kgMD)@IgT@KYKLTl(8c@M|g8EcX4eeye^M0eLH)hGfo_g%vr zism?VIaH$v)@^-+T5&WZ;+~scJimJAjy8lFtODiv8J>CJ>D%+Zc-b7~c!%Zo=N{ZV zwW3@&_F9EQc>9uSC^HW{A1)ky2t-9I z%8_H6)#_HOu)RSyQVnJJMcwdxFmenb)QWQC_=Wj@w(;2gh8fjRhF^?7o?1qZA%t2{ zjvT2IjpJ)Gs-YbHe%qhEMtKMkf?Ad5sv#I+&AQzaYvb?l9YhWr^In8nQH~te z?X7p>=Hq8nLm7UpkNxe0$T5UaE6Rn#&p~_RNi~$22i{aD9DN8xMJvjY!)Auo=aO*k zl4>Z!FX~q2t09D1QH~ro!?ivqhPPtA%I`){1_#EUGpf&ty$H3U963_&wswAC-p%hu zP>%K4?QaW3c?e;DokXn4obTsH?`oh3>lAAmKc>@J7WwutD#n9 zjv7KB68cgEs!;fpLlL!AIp^yOhlV(&y8Gd;c9yx{(YDGD6{CfPx`7QfDU>r*` zs-YbH>R0hY2<%bHxAW!dJ_JLoMUES7C!?BzFh2Pzeh8sflyj}xSgQ&0$airlBM+;a z{5=4xRWCxVDCaS6eayFCfbno&&`<`4jlcYr0vq#Qgj!LK9M*}g_wK@`8P!k*hxM`i zwS^&sT2YQ1c1E>+4u*wA)lde9osaTYBZd%aMLBZV%+UH=5}t+gJm0OM3=Uq`LiX9G L7ok>^BgcOM>&LBF literal 0 HcmV?d00001 diff --git a/src/arm_assembly/meshes/manipulator_link.STL b/src/arm_assembly/meshes/manipulator_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..6bbdfa566555fda5fe5874ca1470c094b41bbe78 GIT binary patch literal 54284 zcmb__3A7c(wRRa3ClpZ#D(Dpv4wEv-Rngn$G&oNXj1x{zoDlOH6-2;@fU^-biN5Eo zQG!Qq2o4d^aQmD_9vBi)uZjkYW8(B2;)MJfMc&?3dv||zx*Pr1`rlfvw{!OX>f2Sj zYFBmN-i0y$fB!G|A-6X(G>b>}GiG_Isq6c}xKX!67hmv6ndM#EpH5`{{okXOl=Bg+ zX+$u3$8%;YqMbRGCLD-hD@MnYZfeWtP>pgZV&Uya2fzXSK!jQ;2lp@A4@PLu5y5hT zh-s`^X{@T$Rs@Jht@QU@o_xY_WDu-jD?})t^(r>)%&|1#K!o%>;&=cDAXK9qitys$ zd4&kIVl@Ar-*eTl6(U$x#L}uUO(mXR2fei*X$#v`bS}cq2G)?3C+oQJSfw0nrR*%L zhs2%88H5)DjV61mau$IcDr5hA1ZvR3Vy_T^R{VRuSEy+~B;%I#O50+D%H&}(B0d5& z4TvB=9)0HCADnRNyLNS6zv}j&d(Zc+eC?4p1XB-tCt0;=1oxi*NOa!hWmYrn!g>;u1O zdGUeUY+IfCx7w(2W*E_B=V0NuD2D?zh@epq^5cR1z&PU=`d*<15frCD<5Y}5t75NE zg9w@>fzBA80}*J2BjO`ag9w_H1ssS#s}6tLHMnXcH;;V|jNr;M#<#SOzdRF7AAwOr z#JF{LEqdAaFh7 z3Z^)HkIaCG;&qN~p;hsULJcBl6-1i({CFS&t#CE_2%HT#OL+Did*?rM#tJovFsHQy ztAcgkfj}#sZ{7QET+YVa(?Gk|{BEOQ@WY+H1A$gNE1O1k%t4?A5eIC!Ps=Hfcl!Hjm= z%9=5<2-F|~`FyWXgNRe!{c&yX3;S8VVgyd1eqI}K_fA%$Cjhp3`mZ|%H~oJ590Y0*p=XpV2O`jlybAKYLMz%o>XQ5qj>*dW8tIqA?6~zWMPuV8H{yriV_m>tOzN~_qF8E$`d2C+r;p*`Bsb|bYg|G(np{M5!8<;zaNM|tKS^Y7nuex+EI@>a6cVfYq~a zeHNYn$?_ZowiR#kY;^Te*IKJ$1Zogb*?)C3r}^?64n%lXcXzDaG3=@wgaHjA`uyPa zsK>$Qe+L4su6?0fZJUEXY|G9>)F5K~r>{l-JodO84n&|;@g9X5M7-Q(b@bj*BXT$p zfmV1j@Mj|CTFQ_1DV}>m8t!M=Y`I*7fK)aMN%K6y`5okqgKFH4us6j;W+(iUh70+GN zAOhElA9F;Y6|Ng&-alwc_~GSORE&AN)Y`ed#h80~)Bg`jLl?BPb{{qOdu7=czG69;?5o>7nHpax`WcQ|`$JuF7}Rz?wyqwY9PpubU| zCT*Aa+R}cISGfqZ0+~2yoMJat3#zCAnMb#c{A?MJ zm{*8ED~Vu^SExb6^ZU29oZGdM!+{9jD&4952;#2qBRbKJHadp`5yqreWX)?ZzaOAU z5yaP4*pCE(R@8nQ_c3>L6AdD0+@ivGBnY&^Y-h|KfL%|AY{ml~AKH`;O5pMxd3-j$_p$Cx`3TuczJM))Hz|W)7T{#uOpY zN@d5f?$+ty)}3FXclAq4s8N|YaNQVFgg`5m9mj;J7laq>eYf2?QKK?*;F>q42!U2A zJC3MQ2}?%|pc6nuyR*wUtWlXcFn5t%0RpX5b{u~R+bd14olP89l~AKHb6^fOrU-#n zDmxCdvn5&eH!pPWUD5NM^c(agSW!id%FID)B{^%^^^uD}E0rAwt=ibF^A%OpsLUL+<{`rsA<#-?$3gijcKK>{ z6*Vd|2jwov)I|uiQrU4(7OlH1dSDebDl-S}B{uKoB7Cbf9~Wn8L=a!W&Z$=H@8nHC zYWwxiKG8gHd+z#y=Mwe35wUNtaZx?|yG8qZEh5lLM>@+vbIG1@SVMD{cjGpe55Mx= z;E6j1MOlPq1J)2nK%Q7DT3zu(|B80=DeGWwUR%vW}1dgJ}c75Qe`v}w^g636}zoHOpyCbu|Xoa!yIYeXUk07jEoUc%W2=UOOeGWvR6|NN@ff_`} zNat`M0HUcpsMPUL$<>m$)1LgbtbmxHiY6sKg=eGb%MyW(w5&k=!E z7#m}*8FGJlS<`Oev10;yURzn6QQy5hq0`Pa9G^RmECrkWv9JBzm|y?v!is6?X%VPF z1dcSF*adMMaUcS%#!kC8c;at62**R;EUg^+aL14ls6hnA-u&)*4VZ}|h^ z_~V~93d7Iet}q8`5aEy2!-N=~gFvgU>pGT?IdfUSV`$9Kt9A&NmF~41s6hnA#+c>; z1X?vs>RBEzVX|=CF{W=g@vGjJ12u@i8Dk71F33Tk)zDw;RzB+5`&(Q;&OB}R&@^w= zkH-o%h!EMX6=N)7a3R84NlR^xL;6t%4y(a-rB9K|KGKg20Eg9uqG%Hh^W z27y+x(uCuBTD9wU>S+S%;kb+t8#VQeIhP;;tz`WR$5hHbPd4pl5vW0gF+WO9 zvc_EP@>K@mTMg<%c}zH_47tBX8P4{@7}TWg5+6VBNIxhiT7=7qDFUs?+lX?aa8ORH zQ%+Q`5)C3~-1wh;pKEP|WL;6Ez-*1`!m0IQvje%tfFT&8vuV zqHs`7jCJm!1`#xm;S5J}H>R9u=Pn}9idI35a$>p$W6Fs-+Y=2UXg$JNlyYJ&0Pv;qU0;eaIv{!^H#}Mi3SlO+r8c9 z#MsO%L|7}~ki0AXr~rr6V7t<%Ajyf`j|>8>WJH1_C&rW$Edn)&kk|+Z<;0kBVg`X$ z5_93;S)zL_Y7ilFOgLm-Sq?;?mCUztk`wEc6GO>~Jgcn+5wcd4!>x}D0%)J=+;W zInkc_c&zZ$hX~?>=jd*;ZmF;7-8+8bnC2Ms(wQ@EjK==DL+!_h4XWw4kt`cCfbD_JjDyKYvr-ct~@M8br_+N%qT3huHMP zE#uO|U)6syW%n30i12gby{~tQKOXY;`tQazdkD0;$JX+tp zZ*zIHG=80EUgY9Y!g+pw)t~oyZ z{5>k~O&JuU1`#rDl6U)z8W8u{VM1l~dCeXItt3u?<5>OIp7HtrnqB$Lw?j38R)dID zQg7lApHAu(Uw_XZDpLmzj!}aMnPcMBFVEgCK4$vz%Hb26Jp@|GoEEQ!Y`R5!_hlbd zp8ss9&UULo1by9VD`K2iuT0t^e(tAVRDSl=;21TCko72e_ng|AdbhrtgaaovdkD0W z^)Fs^Yk#hO)IYj}m%lSqGlSJ2LMs;}f_pw!KYiAA;g^3K5~BtYxUU$~?X`LJ1#5N= zXI*}GJMXd%ATDjm5Uw(RN{k9_qg!}$wj~F$Gz@5mL z5yOVmo4O7TPnz89A<*itZ+4C1&36#5p83;*wX@%C4o9pQ8lwgg#yp?Ae=z1fLYza0 zas$G*T43vRTtA-s(}QJ2(AQ3&N!ukpc$e`OA;ybWh(IgyHc0#Nr}CwhwWs$Fhm%({ zKFK=^ia|j8pzST9joD`2<&|mM4G0H6v`35@M9`?)x+2#P8r_PHE+Wv%_iE5tn}y>Z z{Is$kc|~(6dCNhwJ$V~JE2>QUhs@nEv$qWI{`S+#N8<+bib^#4jW~OPEg3`WqoV5r zHHe_~5zx*l9D8l?Y~>eU{WuI8yh1A+Lu2-uxUw>Tao6y|8-{qJiwLbya$`m7qoV5r z+d?ZGX=6V7cyRdign<=~Amym!O$uevN4#glBu6Pdhc*$$DWlAn+)XrBhl!0 z=3rL^k@?dPHH=hzln;as5eF0H|zJ@{#zXSNycLh>!>h$6GY7x{YnM5kv%9VKnIt z$%5YD_4m~4rytZ`*N5!~BJ_KDH&%2`4E3DowI$!svlY$-Vwc{h%&dQSE zYL4T9Z_laRcE&#O$RkR;e{e>&8m)q0D{P5&&RMrq;E9DZwQUy>k{N_!PnWM^$n6$^ zRv1lVF73CX(tS*?`1ooc&AS9(DO${pFglIw8FVSe+4-H?L z7`<(~ZLiwz&e$`4=n#4~fVXNsV%I)YMJuB#yK~I@IU!*hvaYQn*7A)4kGKSvA4X_KRRi#a#k zbl3lgP=g3PKQgy5$NuEAdbcZ1C5{`bh(IfqT|bVP)H#0mfFlV}t)fO{ZkJ{vKM4m8 z@#KWA@w{z*65c-Vw-IU(p=WL9RnwKb#oc!;6UReUM4*+*j^o6w_ls{{TOveMMUBeb zE@dKq5*DwP3_dU(wac#IjdPxgP=g44E^%J{andnyr=zwaj^$NEpq0wbs{tRM5%0ar z+w`^J$|`D9=5{H=@sqH4RlVk{c&~R?SB8E4S%exy=rf%2>a}ST;txM=C64wgBG5`@ z=T(OCD=rZ|5ipRg`lS|bri$E)tomUISOpY%)Xci$BR8gZcw=1JAJLfA0ToAAQ@!j>N+e@AX z5&9m*d9~gA{}WI7=+DG)r$*4VmDa)56ydzO;k}dNtq)yKi2tm58bHVl6|V+3n)%8d^;=h8 zKnODjH7awvxbIT&*eTQNH_o3DZ+!fIo(2)Ri#o5K-MFRx=Z7yQj={aLEwobEd3EKj zGiwh_pFxNPy-}mGYd76@$@?Co`i**YMts<%`_pccY7jxP*1?huag-fLN2*=&5Q&u{ zoLA%KuC2^DW-NJibBTJZRk1aShgPp9vO^sCyG_Di$(Z=o<#Rj@#fG1@ZJes(LU%ud;WpVf5*Yeg=_gVuTI*7@=ht-C~%hj1JhR|bU- z-9>LYsh$%NDzg>F#+Vm=wRgDRq#^Oa@BEjiK?G%nRG=dsOK5sFfZ<(QM`Q$V8gpxK`M!S9aG& zZ;he-Jaqf{(h_RKD-YqkqWwIS{oF$+nys);be_ECtWeKBbl%as1=RO|5$LTkboL3| z*=J4(HPTxT;k=@=Pw37*4|xbhgIBgs#?U!2)N>-84fQq~T~U?;y`@|->6p-+6R#+t zMrMYGa9+_lF?8p|J3WM=*$Vq)44tV%JyX*;T5tZ*HwBghy`>vcuiPzkXX;@k)X0kR z5Y8()Q-|(MJ;p;Qnys);#?bReMV~+DnM3b9;$1X(%Zc9F=Z}he{#e`_HF*f<6+M51 z?)k%%5TP<#VV{iY`pVkcfS%JsGqYD!_Ca}GS_JQU@yUx2Ws5MsrrIUcqzK8u(h@!0 zRrKi&5om=YVxJ%@?g{dqIjG4)xSrGVV8uNTzR?R2Dzg=ioBa}y_Shz>J?H9|v>%Fa z`%)*eSv<$dbTUp56lc9FtVW}&_aaf|EJC0L5yYo=BoP`p5aC;;-uej2AIa&SR;}JB z_C38q1g$Q;|7-(Zp%spZ?-lK2_Aj}16yX$({qqr?1`w3B_0F0Nc!gFtB0dM54fHOp zRZrCfd@;?`*(3^_HN=7$_!`Gw;tr5t2g$T5g zxsbzw8boNVLzV*(Xod6GpSw7hv=WWm6>llJ*?E=DI7DbQ*MsJ7qZ#p_NZ;MgJwM*^eM< z5J7oGD{>pWLIhgjn)ho)jTvj#1br78;EGb2f47zQN$o3YROWu*nlDD6mCB9-D{@hz zGIRL-C_%8mnTkx`>EbKto7wa9A4ZK0LQjsq*CQKK?*U^H#5eo}?4R@@d^sq8qg z(i=4@GY8H}zcyU0xGl6&*>PaSI%-sA4qRQfRzIn6S1WD{tyFd#4VCv=A=!v%k-J`a zw)@rnh)Ar6!^%lNDthMtYE+i7vQ@t<`*#kg6}LrwN~~0N9C#A~YEfeg@ zWq9qHRw_FVyjuh{Ds#J(yR^pLWw_)v61C#C&`M>;fj6F@MrG%FdVcUL z@72n+g;r@kF3!|=lA)Fg&R3Y*;XCc5PmUXRXY>jY^yRu%PVyRrr#nQT6^>yBp*IBt z@U=R%i#Y7imgX3;(eBG8I5YDBA-YknFyG!L={vl@*8R{(QuP6GnlmEKxUiaAh& z2pPAWejoy^FtUEnQG*DHW`55bXNk4K`1>5Va~ktaN$#0Q_V(;pVayRBb2q2wh(Ifh zzwedq$jnYwdhZ8&1!o3aAE-fufA=jmfS5lYC6>c5^t>`;Ttz#n&sY|s8)F6UJn(I=9L+Vm92(+Sa zBejB$IHWF>s{#2t%S3|+8fmUe6%ME$^blx8-@|hCDOaTs2Uj&ptv-Lhn`jV0vEjN@ z;gGsi>lGrCx>VtSdQSefHqju0Vy^e%(E5a zxGt5ybxt&h5LtEuix^ypuvXF%fBj7y(vLcDSPix-eTrQ6k$z+lXeIsQuZ<-hQme0H zWi^P9*a(NzrCJVb3#}wh!Xb637J(W>$Q%<6nO7METFDIMFHfl-ygsB>UuUA#AVSuP za-?B{t}(-i3Sn09^qUf_m^Z4Xhmz2t8RpYG6UaVViBl8 z1mzXIhsWhazP}`cKr5-)kbGrtBe`O8zQ084KQvz@?FzcZ$%&!V>T7L=)gVG*BOG#niRHky&`M%196U=xcgu~| zu~-cvWR3}k%qz=*ZK0LSw{nsb`TmlSdyX1J$XZd3^nM=?fmX5xg+uNyu^gyDgsget zko!wQ$%(vbZChw1>t8tJ{t}Bo4I=O@MRI>h2H{(!XFHn{?WvE)3Qv8AAU?joMEU`h zFCpe7_y)49f3K%+O(>U8t$syn^*sbyVdkTp_)1)nTKy2)MLzv&xZ|K&{fgA;hp0gW zW+hvzueCBB0W5ORAEE{k`uBS071ip8QmY@L1`&Qvgc>XlfmZPCAbCZ# z`k~b7hp0h>es|!!qFVh>YV||ZAOdrxt<~2WEDwQJ@a-UZMYZ~&)ar+*L4LJhyz8xg5s8&CeT78}E)+W8R7 z1nw)gR$pt8Jp@|8cYWj))#_KIR$sFye}`B`gnpyryrNqDiqz_ds6hnoM7CC6tI0hC zTEVx9E1_tT-7Ksr&|4* z)arW(v?6c0Cen_ftun1ktv>H+$vX>*f!5lU5p8Sr>r$&9q6QKAb)$<1+=SpE(8~9U zYV~8O)emVdCGRq5wkPi#Xhms-8;up!>c>*6&nqg?&|1+OFUr`Gt<~4NEIbZG(E8BY zKH{KS{kqiZdkD0`F|@V%b*a_&Mi&vZZtT~lZmi%g3lD)-IMTLOKbBhkkaAS=zJ#)9 z@?L{7wN|)MKd4qemRfzz)QN_2u>Hc6J;9c2t-juo;&C8?@~+nQ5eL=k$5N~BA0Xksnrirg9ysU_A6F5R&Ym(hd?Ww3$|82mRkK##;1)>+tS;JW#Nk)2i5AwQmfDV z2lvowXm7V)uCf)jWNY>H?j4T<5i;s+j)Q9TW2)6}rJ8CFfmRqzW2ja?mRfy9=#I?M zwC&P9uebG3JmBsf4}n%V7i_J5EVcSNx^{eI71emGWFE^{QLTO~wfek&@XE0oIy36c zPi2X$ZOPW^>zz&>2O=bb!a=qAvDE5&2(-dz+FE_Od&k!5>rAxlh@kVM{r5RtJm5|z z4}n%V7m_T>wfeeh?f6K3w6bKln&Y5a{kqiZ^KPJ3>io=Mze8p#$t4zTYxVVZF^>Zg zk{N`9YV|o^ajm|GKr4)%4y?8bs)K&(15V)h|n}zK8Iw`og<2s?{$`tv>CXsm4ci z(^`F6QL;O8t-gn#NF`Pne_N|xlUjY<2Q{_{LciH|{eW8vJOo?y3YNd8rBZdb-l zP5@BN(W0j%`a-$E%AG#=cU!BUsmPzR}A>5i;5k!5{1 z;Jm^*Nb$-d&`M?JRbCw=Yt)L{l~I?S6KY014I=bCit`FQLTO`wfde0 z5&C}4d4)B+*cMu;?7X5{{m|9w-=0+cvPP}AUCDg1pHr=VMQZgu4I*?$bzWhmH@1aV zDm$;JR=?tE^~WWZ-mFn8ZWs4mTdQA@T76H02;D`US6JzdZK0LQ&MW-xRrfw_OJ%lF zgyZQ@@ zt$yrk^@k;QaxjOY*$QK0YxQHP)%P@rpw*l_$=F)`_y{mdpevQop-d3R(%f`f!^9${p6k^)JShVg!3xzo+9>2(cqQsldaX)JDmbL8*25kt|-fa z-r8FIbm@MUBeb?wYR`21ic0S~!XjXr;2_n0Cv8`d72g zBMx&PYE-ry)g{63S7!-FAp)${i0nApZ~LZvTZic}6%LnBqcU^En>-s_eblwWQG`G% zl^w^4gSQPY>GMzh>G+!Suv^z`tw4?m!ouPC8LW#;Jf zgV%!|2cIt-MF_M~*>UW!enxozJM(BYwUSx1`KjT5 z&Htpr|8g&CRA!EsyQ~i0J8Fb*6d}+`Wyi7PtF6P9`A3EHR=hWARA!D-mOT?3J#wgU z6d}+`WydjV|98tToJ6;gnCtFCjmpe%m-anlA1M-c+8RCXMPzi~-;@Vy(;SS>A~ zMrG#c+`lzgc>B?TkC<@STZ2)9h^eD~8{Bll!hq4nFvmfbV{r))Xr;2_`1qE%a^<+6 z(`>)2gc_B(-C-Bb3tk%0CLBcwv{Kn|?0DM2;qAZaK^%)qs8N|Y_MZPpaNgu)!cl}k zE0rC`?I%wQdw)EDR@BN8YE)*9iElg_9CGUO!cl}kE0rC`%EKpw^L{jsA~?5%8kL!2 z?Tm+mE^V&~M-c+8RCXLazUdntKXN$D#9<}WsLULF=H4HiaO%6lQG`G%l^w@FdLCC9 zeW?3az)_e1XB-tM>vWQ zXr;2_`0rPT#f!TRA&$jW)TqoHokv_5Y`^jq;V43&mCBA|X?;q3!k05C=IvF~sLUMQ z)?6Aq)w^9diV$d}vg4pLLo0oSMg4gHKGHbnX@*&`M>;L9@N$*2nN(s8N|YXstx)8Z1JfmCBBTR&Cv_ z^MiN|+O}AuGIP+HkJ4OHgg`5m9S3Ef*yXDQRn(}=9F)7FG$$4z&`M>;L0L3*d3RM6 zH7YX)<=`mI?L`Q*QrU6PZV0=T}jqGIP+L z7^Qn{5dy7Lb{w=j*WG^J=|0q`%pA10N9mcN2!U2AI}SQ!#O{>QUPX<{%t5D*$UDiz zeD)dRPBMuG5p=4F((@H_xYH8lU1$rfRCZp`iL7SNa5H;WQKK?@Md!IFJtr0+&`M>; zK_|=DokeF?QKK?*(77~9&$~qkv{Kn|&;K~G+>dsgdI zMUBeLLCKKr59U2R%X7>@%mit~Y8_W)6D(jMC@YA_Q8g>^SI2xZ<9jr}J~I zZHqN3TaK?6M(Oi;Ap)%E$=f0v2R+e;?j6A6CDf?Q9P~aQO5ZaSA<#-?$3gEhLia9X zWeGJZGl#wjVVS)R3HGb`7rDsNIuYJOokWcK~X|8kHH%EiD`u zNoL4HpcTrFBby@^p?J{EafnT(|_`q$}Lt}~%Xr;2_c;K9E!Y&W; z8_0V~s8N|YR-Q4wrG5P6!cl}kE0rBb2X~upY#!=WLXFDIF>c*mi=I3A65%L9pq0vw z<1aTnTHohnek1(G9Mq`H9E*A_Y8!d;b;41EKr59U$C-DB@z0jb{zjrG2i#(^N3@n zjR#w?MyW2((h!arCCS+n?`_rnj}IQJFb-9yg8bC>%uyv{Kn|JVI-5 zn+JbN9Glto!ELcdt(b$?WB2|W3r7(GtyFd#70M;UKKY0uc&lCK+!kxpia9Xz8B>Hn zE0rBbUKVAIS}_OBF`H2x!k&3d6WusHGgM91lbO|Ywih9I4yYCLajxW+?EXRbUpo$t znr0au-88GEXdi*?B7(;)&AaFoBG9VXtHyPg=8x~=6|X2oU?wW=2U_8X_z0ZgxT3Is zJ_0p}z%^ftKr0*(AAxfT=Pq*l2-F|~S649tt#CvvBDpD@p3m&x0eM!_^Pv5E9m`E! z-w$?ee>zHLdxFS&&mf58jfKxaZ6z9g&%g+MACTqHCsWqw6QpwFd&L~8L4-aVW;qan zR>eI>4I=c}Fqb14MYPfoTb0`9>a4z^9G?(n(8o4d}J06XQ{X1ut12u@Cegyerh2u`KvELVP zyBH_mE7Tx@;%~nk$a;kcv|_JxM0^g^AcAJ4{c<47;UT~Z*T2t!*?{6<|H{toVoN>( zHHe`2+rOW0*ekT+mh$6)8br`Mwtqj*=g_v$3g-g#1OB>19LdU2`Qi&cX?Rw5iqZE5E5rZSlMxmKJUcPbR5)g z*6NL8=7=ZV)b{UDOUj@0A5do59RIQPYTT$>BL1Cj_-sI^1`#at?~JB@oNEj{*-h); zI_CXt;s@uRUt>*t-c}WsnS*8Gj`;Uoo_wMKftp4Hqa6Yx$OzWxSg94F{RpD(d84q| zc|GXNiX4hy4I)%#ZXdDR#L3pH!`C-e{_~U@tUTkTohw^Szst%mUcE~L$7e_O3)uIb zL66D@oLM4*-ae#UfrLWcE$8blP2E+WwCptly-eq`f;vq9yjPw#AJqRt+_AJlH7 z8blDc{XN?cM4%OUm9#`0-;R{OhJSb;#^d~v*I3{COxoFtjgP<>AfoNi6D_*_@1}+b zA_A>&zWF`J`Ln~p`|iu55KSBKJ-tE%f78V1*AE}eUO~^%E3~5D+Zd5M+d-4IOMHve zL%$!`7FtpNY)jeoff`y-)=JhOaARgb4n)xC+L-fdF7^toUjLtw7V&k@of{CS!MuXp z#Ts5l_ z8O|DB2fRL5<`u;&jpZcoCJ1I{4Rff>zO%RdJ7)>v_;#cqKJ1MeMDQ3gBAL4h2S<=K z99xb#%RDoR5vV}~&oM^xtn_=12(;ok&4^^SCth*pWR0ExSjH1Tl0`EJ)^Mg)gv!az zxj_V5VPyS&=;$_{6ZCh92b_IS!#S9};&Xz^*?#09*h-_BI}>fYtig6Uw=-XsgR>}W zc-$!-R@VKTf6vYmilDV(4I(K1Hcm+{Nk$iA%NdT_l}v48p39MF5JCSIcZtzSrWTG5 zd*i%9EA-Znpsz{WCBA*t!(s$l;TZY|&T8BipRaIr@mY(1=d+fNKn)`N{!wmEMi&ui z#o3NIvbh8`h|qbJtj{8h zkX+J$P)#0!(Z=+8bcfh%|M}dWj~@|ooSB2ynj*Me{+(NLYp@Za8bq+H+%$LjZ}ajF znIoti$j85HOId_!8W6r$q3Qc|U+xD-kU2OzBOgZA_S{1-M*=Gh`^IYBZASA+hLZd z6|V!8nS*8KkoD1kP)#1f&E4JYy!v`>_s5TjdEGHPXBkCsySN^WX+$tbBf|G;%e6a& zX0w0yy?N519tXCICntIkEy!5njFAJ|_4mOd1X{7o-nvuYZm~k?z_emA<&9t z=HL?t-MQR!No?8=Z=Ez`hdOEyf#-)}gm3i*JV6piXf`>#A9LVSBmXPyJkor6A-?1s zWI1vXXvK2c59)bnb{^J`#|kxwpmDQVG#TAo1X{72#)Bdlnnhjuam-PJ2#UYua3}pD z1X{7o91cM(B*yKjAJ*$m&Z&-y!O#Hc|8o(3r=79h|HpNi@3wmGeJ)9L!T9j=`a zqXrRp8Z1Vj70cWYcNV>TMgQ3J=+Ju7!<85{h@c#7MSc7xD#axtf@ tZ#A-KBv$-8N7n68jR@6XyU53KIL$`S|frviV)7L{{>(J7XAPL literal 0 HcmV?d00001 diff --git a/src/arm_assembly/meshes/shoulderBase_link.STL b/src/arm_assembly/meshes/shoulderBase_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..bad98d04de53b49ce43712a08b7c6213f7d6b192 GIT binary patch literal 53584 zcmb`QdAJ=_neK~7`K2--A|9LC5PnR9A`K+qr{!UF*WxfsGEgUpB|hY%2<5eJkZ z0>U|f2)Q5vwvcl+Y9pPijX*n~v;znNQ9zWz+o0a{uG+uaZ`ImWKl)^}U)ue&eB#Fw zi&J97`}W9}9Pn&ZUH^b@U_ii(u65*H8SHD%LZeHV|o!l0k66-zE z$)7oP%VEX!d>`6=WM}P3zJt9^eeoKDScX!z-EtS{&pZ}8Qi;kI6 zX@!*VQaSPP7pCXuJ-PO9JAq22I{Uy4^Y^dzt^uw4>}}J^l05a$)@9#>*rN+pzH~qi zJc{dtB2D?k@EZ`QL~D!<&Os|mwf!oqsojCkAYd7?V6R1SfZfOoY zQaSPTto396w-cyDH}{8B-<RFXAZ7U~GiT=?a_sCyA|I`5l9;uwr3?4i`oIoY|*4ICjU-!jB2IRmal@qKnX+4}k zCHmv%ot$sC&#VD`jz=mdnpbKkP>Fu^Tc_ryt##Fa96Dxjl+h~6$ba^;uMQ6$?RXUT zvg>9TQW6R+P#KjzsMQs%5Xa6CrlPQO#?PN;zcf ztVX5EMz?+ZtnQXyJZ$3I8(lcQ?S>oW3ol!`_|lsfR_r@Juxb94D>r)s0+k|RE5*p@ ztp|L(yYZYq#2k2(!|kn>Zkf;h$>BwRG!m$k16w(=Y?VdB-J_qI+I5d|ppyOXSMHe4 zT5^6-k46HOB4HOfZaH$r?q>gXK+J*1fAm;;Z;mA&bYj>zY4_O)J)CIXdm zU@OPS*wlG@=D&UNQ!xh~k1PA?=TFL4Kl!SdqnSXZ9N0xYF1zE2{Q1uw8FS$AxU$#y z)j9cBZoQ;ek0t_@a$pxZe(-^fJBKgbIp)CQab@3n=z@I3<4^15Xd+N42X>KTY=!+h ze?4v0m;;Z;mA%E$7v|^QuzN2@6M;%Ouw#yFi*J0g^Qq^)9_Jt)k1KoEB^TxEZ&_ST zWQ_zW<-k^sk?X&+a4f)7cmDOk1PA2|G6Nax$QmiswitDP$>s? zFULD~>zsAjP{%zUSN6pR&d*PO?tOQ5XRgsXZkK&y4m=)L_Qp@0nUBm~ zQsihPP$>s?QK|>lz9ZlMO((@1cs#D`v5y^>zi`2ey?QhesFVX+IYx%wJa+%^zdSfM z=D_1|WiMI&(EQ+=UynJO2~^5~UDRWn$s0`Uc;lyI4m=)L_6ke(&Zn-jv{#QN0+n)L z#~eeut^DxBZnqy3bKvo~vcG-JF8Q=S-_z@lCIXdmU>EiH%|q*T*V7;=KYH)(jwie;=D_1|Wv}t;&GKs(Ozq8&CIXdmU@OPSl$Uou zp*ySlW@wl?rykg`0?x&_MZYNMF2X-XJ-g&@wo%L?rx_i{r+s5zs$07N3!zb_1 zS@gY%{lDKnC;!_@o5Z#nX(Ui761H+=WB0vqSZB9IuT8i|IZ(+S+vxcG?&ZEy^hYCs zN|CT*Jw_kB^Nh~s?QI8ka*|xLu(rF#{cwE`*eCv?>&Bxwe z|OGo|MmyH98Cl&<-jg-9F@(R{`SAli#hOkT-g`@b?f{G`#sRh(L|t94(uYw z!>6A>|fn%N^WZG;M{L z1CPg*ec6Xs$Twf@f?kd$0+n)LE62!`|5)R+?&`06Db7JW9#{6I8%)aQJiUL+(M+IH z4(wQu$$xruUiZ~GD-XNJUO^P@wo2d@wl?T z{>$a_oBw`)v9>o7sFVXcw$Qf3vkN?7h z#d-T!6M;%Ou!|hs9j~4^VOKE5;nn1S;jg zE^7X}^A_ZPoN-1s>;?%EKb0mbyE)}Ki6Dx+cdTjY^I_~kfvekyW9{c59jwS+?B4NjR3_Tor@cGbQ z?(w*?)w5Gh{#=oxkwB$L*kz6ha^Uf}vX@?dNWUJ+F_1u|9N0yFgqnA0D?A=o_Sd#O zD7JJvfl4{B3qm73jH}p#cs#D`uO2w7I<5v1sFVY{k|T~?JRX;P{rS-=RYItQQc;gS z5oT1EIf%;R%2pd{1{bAjB2dvEB{3FOh6$C-JswxK#)4Lin4^(ErAXK%F>v)kr5vm= zMLk{(z4vUdRP4}eRkJ&orCsC*?bK22l^vB6vbhJ{PM}gGY!Ua3J{RWSW4-kXkH?kG zou+mIm2zN5VoEPZ9J_ctF8R03Eg6d5sT8&$P#XKD^kj;T|VKsirGOS}G^1s($^dIYQ~mnmZ9% zA?8pkREdc#Zi=<-<_}B{`Nm^O!V^0eF5N2r{?=n&nqDQ`I@HqHH|$$5lj1*IoBGVRkytV-Ce~m9JgZdbr04tJCym!c{gF zA~DkKaYKx7=12IQ=Y_`^G18`l=7eY`WYbnvf-__hI^)pug0gu+FwKFdPWahtH3w&X zA~-vfttYF_Kjy*MAI$_RXs#xz1bZ)1`F;sc)}{5pQzv})qng7BRHAu8FwKESDkpUP z$$m*ShZCqobJdsTpxve799}m4loELA1TEc8pb{OF4dV)rR8CxU<(u;pclhFfKL6OM zFAeYe?O(;+hzB zus_pIpc2jBH>3m}shseApXw-a0+nd~z9G$lM=B?L*SVU*2~?u_dy_N=9;uvQqK zfl4$_v8MzcshnVSO^KbK*eGX(`|kHQiGML5nk)U3z$29tiuCfMD-GyDCs2uI$0E&v zM=B>)`po2f^ym*3&mmXNOPoL@nqzyK1CLZrXjK|~G;jiyXpSXm4qxGvgX0IQYf9ju zR1ox2JAq2HRy*#oRA&?(shpsn(i~2p63y8_O5l;o35`njud1bT0+ncHbDD$Wwp2RS zYGlX`%4IkMa6-rV!DE+Lii=7%NA0v!cr>+usoRA%&mTlEcpbA$RaaSbG;U15x-=VYY=2ef3vv%`qpUlHGd5qQD z#VOkx2Q4mQ94KWGhEOOj@vDLOGre_gUQIM7Y+vUO9?gwI-sf@;#YAcdz=XUA9EDrD)jk4 z!d0@Bqa2Bm(AW(lE_yT%T&3Stk79m=(Ox0&IN|w%-{!gJ@VLI4P$k^sM3|3ZCQ&NS z;e@O7JL^iC1CJB3`8~`2dhm9!=Ih+Fb=UX5rBaJ(2SL9pcCp`HC-6AY%a@7Hve#$6 zJ?8lRY5P@j^x}>xuJ+qQq|su(y@_yDb%*|dx9k~5`;$vQ)pd^(Vcv`?hwgG@SrbuJ z_4l$O5$CybNTv2EV)f&Oe2e?_2ss84u9B@tWz9qEY(#D49w)RjsT@Urgtiiq)d^S0 zR*tdh*J}((j5|^kC@om;(Zj$L;0ojS?wBH6j%aSf_Da?N%dj$sdz=V6Y^uj-Wp^-^$_ZETdwJwca>(O^ z*>Ptg0KJ-fELVj&AM2FnaE}vKr|F8Xe?{v$;VNsx8lio|fU9+uDQo@Rkeg(5BE5s zo$a6-&{j@VRn0Z`<96kEUe24MOw>G#jM!@$)%1sEs{LLYiSBU%I~|FG2&uvti=!mW zNBuYcEBeDd9@ocPm9QMTYeihm^H{%P_3<3;al&$_U*f-_R8F`mtXDB#u_ij<9w%%} z*K#=Fs<3Lu9F@DsakRU~iO|DSLRm(`{Ls1;{}uIcqN?g2eX2w#yUH0?(a;Kt7gtUj zm9=`f$BEwB-aFzHS&C9QQB^h7!^`f3AK|L?aH6Ves<|Ioe9rrksG7q)PWX9gGvO*7 zXM?hL7Ot`%9G-^Wl7svUG#|hb$9FreeZeC~lv^#Ydsf^_+{q97T-M-)1ojt$*{#bK7b%Iui zZ8f@5_)Da(%{*7n6jjR3n|r*3S$5<`SB<~__MqQzs!n9tj04`^Sz&C$u6vx2&2DrtN}32% zii91BvCD6HJzsLuVKE0Dk1Lyf_;v!7a$pz4!Wp;b_pJWOm_t`4x?|!h<>2=!;hs8C z%@Mh9-XN7`C^?9$Nc>*pfN+m!C$OV-=(#m-%YX6tlR8qdzYC!wry}utm2gj;P>%BG zJoNCiS0IkH?i=U45DeRLX&^9L2FC z7(?=~|1DcPkh+U3+iPAW*r$ZnKBa7SCes`qmwTS=Imj-R6Wk?Db2x!YG;bhE2|Q9c z!9DMkU}u|BQFC^qQvy$&pbgszRHE5iP6<3xInmr7>=ldP&X4ToQG%yVaEB%>l@q8$ zGp17lk5o=DLfZ*cqJ5rM$1WbJoM7~%Ii#YWz1^By7Z3H&v#zpfku--Bs6_DGK}z89 zxUxB4X(v#L;E9Wrz~gacb9XT%e17ouED_B!3XfDyRNk=_&)Gw^xL!GdN;I=MEfpTA zc!Qbj=03+$Cv*o(Z)Hn!IDtyEj%pk~sv{APR8H^~xHN~iuCFV!TT0+j4kvh0p`Ab_ znzyH=1m%^Aw;ai)ekp;cPVm;Lb^;YSbS^QN;4H%l-k#M?pi&OepW3pi(~UNche~>G8O-S-aW^ zRLX%ZVr2BBkDS=Ky4YbUJswv!Yj8V(N;$A&sjffam3f_?zkYH2TM|4TS2o9zb^?`h zV8>ETx%-1BcP`#H?!%QHk1N|}?MNemN;$A&j!rCF_`5ZE7;&;08L~O2|RVe#}$98R@B1@RH8Wsr#b9gw0zP@1n2N6fk!I4t{vxmelVePH4(P9 zmo1$V-nve3ENLfDDPMCBdVknabQw$K1jonrQlV0*INGHIV^k`;(ko{{N-%Tk1ZQgP z1S-+Y=9IuAl@rb5$_Z4WE9+Hd4&tF6I(w1LI8IAN-%4d?;pJ>j37_XqaMs^Wpi(}r zGExGMRCdL(jHPlykgw`(Jky1V4;E~Gq;VOSq-AvdnUdx)uYN;Gf zFk;fSasriTjw>mFM=B>cKDHC6L|3j@D&xvqx882$-(Xb;+ixxVKsj{uDW8sRBv6Ua z(XRaas0!gTRU=04eGt(+Yw<|sgxW2AJHE$~jY_s(O;kq-o;tzqaM~YEpc3s@Uez3U zq;i5*PjfhdN;KD^DS<~SCz|`i2~?svGe~pbk;)034Gtb9PM{La8HaLYLqEQMk9^`C zXX~CcWcY_xF8`09Ea_DY&>G8O-m9O08Y$Q-A61H+= zW4*TOy2sZy2%|mbz~gacbI#aKpi&O(NT}vr+6s@ym91lY z*&mGrD&@c~O0`|+i$_8p_jp{{oTIiAs1ylX#E3>>H_VSXcJX*z*__F?6R4B}I})0M z$`R)v9*;}D@(j6=Pzj|{4s7KJyR=1Hb=>1|Wpl38PM}gG?1Bg@T$eQwkH?j*vA{W8 zToVTpsFVY{$nl-f7g|r;<8ftcOmn`Q4K)#{6bZY?(R4IGr5u`@)MKPPeuUkcGW!v$ z{a2~d?@siO2JUe}w$>QF=Fv=`QY7qHkMj889*--Vb-tZIrAXK!`o|CVcwE_)qk+b5 zJmS;|RQeq|=Fl7z5uez?<8jHC9zQ&mtH=$}KN`5l3E8aqv8~D@PMtud-?1Z69u3^% zab>gSw-cxo2|Kn`dHisX$Ca&dT;7RmBv2_5c2RSVAMWwEvRU)n2~>)NUDiAvKiuPS zWowN|j~{gcm43%AO2zTRJswv!YkoU{N|CTd^p79z@wl>C^V8v){_(>-9#=MNzDm_hpi(64g5db!9*--#dNgPvP$?32k%Oawdpxde*7IMx1O$csrqzC{~^DIhd9T0u?#RwWPe- zCa#WNvKdzpgX`g?a-uTYD>^Clz_n?ofY7Q?ID&_FeUM2jf z#_@I8OXY;00aSB%sZc2gEfU9WrFA*7NX3zC*-PbwN;tSbyi}-^gBFQ%P;YOI?_$-N zm#+Ku+=9;A>@HB{nTagZ+b{KIM-irSqH?dOnXocBTa*9lbm z9lMf4*R`d`~D#xeK6O z)yjKo2NI|h30tM=zX{qs!LHoRVCTM_K&437ktpBm>>lMnMLzBV^xp(sCu|Qn)&rvd zUS|=eiuI66IoRb^shSB?ii8~r-lnU4hSKA4WwXEEPM}gG>`0XFp?8nRmCap;b^?_m zVHX5%f_9I`mCb#Tb^?_mVTT5>mhRwqnJh9_bxLJZg7rsbte4X%1RZDvkiqZ`^}Uuxh6{sG;Jb zB44>umm`tv8V$N$_7Y+%NB=k|0*@22DPcQ-ic-;o{o|l}Jg#i&q#VrzDn-JMM0p%^ zkH;k+ZPh;x)(O>v(T*K!UVagQdPwDQWvdO#?;A7{s1yl1=AZ{V?(w*?H5M4V{ckAL z2~_$WyQl|WL~xJCmF=TFE03IY0+oKpF55~gb*u*-k1Ly1qMbmc9M~fI$3gdaT-mHE z?F1@C!j43F9CVMzC0}K<$Kzm~P}!xTv=IH{pnE*7Y_(x}9IO+l^gFid(f=ZXdpxde zjS`L@%>*h%!j3r@yIuEqT-mHKBKm8)zLmiD$7J(0Fxj+QO5mvzUi0Q0PV*JDGzT6p ziEJMw)f`Ts62aHt(j0g^u5AB$T{Q=uI-xqT!Zj19MDwk^v{ZPca)K`arUafkq5fb^ ztd`0NRHFHkVVVPvR8H`H#gxEPCwxt;mdXiKqWMl_ngfqiPVn{0l)zIb{Af@ul@q8$ z^A*lC2Og=M;M<@nfu~ORaj;q{Cs2v@ub);4pWA$qPek*q#Uqsyd}}YQhZCqo`+8L^ z6&|Ua;9GlX4ku8FZeHQ=Naciog|AvFCs2u2OLP3F60|!itp>9Bu5DTmJaxj)uc|qm zKqcD0+*>8Ce*6CUPxALKmWr<`$5Qzh3(LK5_tXi#kvyOty;vep$!=~dJauBszP((n zhZCqo(+X*+@JQu^e@VHT!wFQPn_G9+nFr=CJ^1c%sTMwQV7~uqJ4ajlWw-1yx%g|@ zO6%gO6W^Hszw+14+4T(wRHC<-GbiRwYmP@MCl0#j*!;xbZ8so?6R1SLdxuZtS5IAG z0D(s;C%(Vl$@!$WtosH8D$&Qic0&I5DUZZAG*$b<$F9%8b#K@w|KvfJ4IuC+t`oC1 z*(2Y6t&84(KqdMuckSI!Dm+p-vElAB^7U8$`G6cwpb~x18T&T0E*`0zm^|f!`E@71 zeL(9vfl4$ZG#w>)q;ldd`|XooGG+4tIh;TxnvtL8@E+XjrGxW>XTPPg5~YN%QBG*y zT=3IuKk26RagHv{2S8uc3E*`v%<-)jKm7Tc$JFL zE}NcJ=>FeS%fs1{Y>sNOIS)z+JavMznw0R8FwaFdmkN(mPOuWCIh;Txnt78Fc%*WI z6}6o}C7O0i2|Q9c!P!|mfl739>w2GiD>RQ1Jc{cCvpFr56R1Qp(^A4~&Q+KQ&QVeV zk5o=@Mb=KB63z8hN^tfn6=#{UE9X9y8O7|B%I71qIn9BmPSA$!1S-+2F)4vZDkqxz z!{<3WA)@K0GzT83oM8W{L zni6>G1lQE<1S-*7Q>O$Tskka$_EI^)EJ$;BJy0nJ*9j?sM=B>OGpal<(KnWlp0j_x z=Q-Dm`!}6s`*Q&OGh})0{J^I9SFYSF5?d|ZGN1dC!=vp_CsYYMqMi8nD|gIiEjhn- z)+dz{sMsH$8)E; zRGa?jhbn>`RXTMHJ~0&plUA_<(0~bHGXwY{*_xV8IZ#X zRHA8-v>rZ6yf?SqaHD+TWlL*keTs!gwiEtjO|^BMKqZ>5xum7SBb5_;!6zl~)CrZ4 zr^2eGasrj;HLut>zx%1F18RQpf%EgzpSxhXRBCDct(|Otj;xx4=hLKGd8Lc;gO7Ro z-gbgmb>g7^xgejp?LEai&nlzc2~?sfZQ53h2C23<`ojF&8+NbEm6Y&V{%D*r_Z`&Kz}%aN;EB<=Acca+W4t6^O4z0D!rK!-p@{q zeeAgWg$rJs-cF!WKF$C1FLLG43ddM_+njClonDw7OU0;6bKnu}#2UZeEWdWa)B&T! z2~?t+N4uAO$?}Kh2jBd9ELC$o@F=bmD=gVNpSsG@0d3_3DsPdr=H6CLeEXVR@@ap* zXFv`oP9Oc>Tr8h4uziTL)uOiCk`-&++E4oUy*0J(3|Z9D$yD}<-Sj)2k}Vd z1fw#|;RGtt&25E8DkqpXX$~h)iKd@Yf;~&An6KeM4*z* z_w`Z&Pn}?mX(v#L=DaH<@JQtZXM-uh{;mkl+hw!2oDz8I1XmXA1S-+&k*5S6shnu8 zhZCqoGt<%>c%*WIc55e4iDm|;1Us8jaegJ6J1HrFr%rHg*G`~{wBIXAR#Dxll8P%N z*^GrW2c9~?bz3`uN;G3ECA{ohaDLrRpc2ifObL!&QgMDQn=_7-z*8rm&CXT#lll3I47zSu9A&P zk+36?z3)%YP2`J?h&k{$A)7OUb^?`hU>C$Xw|%60-6vLyIq-N~*{qfA1S;jg7BMn8 zv{U!#(8BKVxUyNn+X+;Pgk6+sr$bNd-uJn?Cfwt3WpnImCr~L8b|l7*{oW?sBd_Un z-Q#g(vsShfs1ymiAhuZbw284x&x|?ncwE`6G3^8@<-jh9oE&&Ou54DC$e z-`k{fO0TW(cwE^W?b-=c%7Gn;EcAJYKF8y6WwTbc6R4B}TSOeY9mXymk1Lxsrky~g z9N4i`L#y2OkUWK|^6GAAg|lxQUT33u9qpKN4^o8N+?7*~kwyZQB4HP$`us*q^Iw1c<1q&w<#3z( zc}&}v*jy`#$zk?VtHKI+|z3(P$?32L0qxp$2;df@`nlccwE`sX>2D@ zDH67bk+B`G8|tjP;%;5{cwE`s!E7f`DH3*3stxYEGT&mWQ)3Q19#=L`Dzp=*lmj~w zW4}G=uJOJ8J~!sT<8ftk&$FFCr5xA=@ug2cH*v`ikBB+&cwE`sS8pd!DF?QQk+GRK z?ACqxnJF;`9*--VCnMSkRLX%JiP2x5d1`mjvzNzlg~#K{=J|4akR!VPvB3**p5yVjvUz@@oj|1=*aflfQ73h-cT*o&9I89KZF;4vT1ZLaIa=p!I=l^0?@)A)`U66??+7;&s1yl1mTKr{#ryhV z4m=)LHamLl1S;jgE{Gq7{?@xD-Q#g(`(9r*)I^|CBk1Ly<^mYQ3B4HOq=<_apj>qH5X2-LgK&2emk%(_kx-ImHdpxde z_Ile1REmTh3BAFrdwcN)v(n>n$ydzUz%&x_rJrt|Ba(+-IBoLglt+tIYx$-TW{@r{ay|yP$>s?BsxXevN6Rv^~Hnp zBQN-C99Q&*e%ICZqEBYECpp|B+X=6E|EZTcfl9ysed36iJ1y1TYtG7_-g&e#+C|*{ zwgdB(&-h;L*$?-~rgddg!ZZi5TqRq%r!9JXKn^^0f;LQZsD^hP^{=rW^nqx#?swm| zU_cH$QaM4L(i~2p60K4xcS_)q$_ZLMCA=O(R~=dQ4JAwoJj&t3_ivcf(1T8(63vK7 zbKsH63FRBygHE6lT^WhxT>#A-=DBQW+5I(9bFlX&wvJ-aA0OGe8{^Xdw4v&e4ZRlL zZ1jilKe9{ZglyWaoj|2X*pZm>yBTM6Hd?SC9zXDST-mf+JAq0$up=?r%Q4{|k4rxK zBWom7LaC@nNsRT{s_Pz)E8BZ8Ya~!95_YUd_UG^}y`?kGh&k|hT-oYb*2JulK&2em zWsV7Q;PJS!eJzPO1`?>013TtW&AVY-#d_fJxUyNt+6h$3fgOp_zl3(WJ-qYGJswv! zt7|)fN|CT5F{PIyj$J$+mwY-(v=gjql(r-^2h~<_4x;k7vekyHqH)#^Bv2^_wn`OO zh6%k9(LEklw#GtvG|*ZykU*tK*kumAi7n>9<8ftcOjD2k(ODk)R<@5AMM{sg#Fb|d z!ThM^AQrS#vYBbI9_4unaaj|+Z-|r*1>?Eh>WC?K$d1+2@s- z6N|V~$*zp63PCGM#VQK@#y#kSukF>Ed#O;7ub5H4cxbB$)&Haa{ONE6J$y8nqcc~U zG%UOLiqfRv3zu#+UBAEen3uwTov3=Mgy@3!+duvhb3FR3L%UAMMm4d;O)*C|e<1wV z3HQ_q(FHO0J-dbSXW4fy8R|MA8`TS)$77E0UCZ!aC)`sfL>I&zm!1@J?7q%z6HdrR zb?)j}XuE85lbi1h&+j_no;o4AAp9!T3E8OZ_u}tTW2r*cA@|gY9vuiPRo+0{YxP@P zCEQ~X5&Aq3HcC29SPw3?@m3?;Qzw)ojNL%kXzw^- zBQdB(rl`-uya`ucPPnH|6m8Wb^1y2jcASunD$pTMoP$NFN}}qi68(CFwu(fUgQ`av zOE#*|3i0={bxXoMbwYGOggy`Dk8@D{QO1&uD)eUjJ@=Q6NPq(m>vQ?>=aQt%*o%wMj$EC{!V#amLmT zR(`z7DAcsYwgHn$8~%8=Iu#WoP<8ISJ0UIA+UQVgouSjxN}&pag7~9T67Bl#y+8Y` z^JAZxKT1Mo<*x5%t+juiea?O7R@MLSe_J*w-rBsX_Age|j?whB?_2)#$p^+qpZ&&) z>^<-Q=kZrBek9mCZ+U3^x*Pv>rAzEN_42(t?|3$-ww-_9-p^nCOeF63%xv=BZ}$+y zQjUGEKREl1O`i*d)w2GE-B0xqDV7MI!>XOB+HO@|wdd3Wd)NOqwy8|u2`zVoBHg(7 zbWaXXpz>|%R^?mk3H9Mup6hG5Cr~NJ@%7d6XV+#QeExqz?=nu^s_-bTC)V!%o4sHD z?dN-1?g><}s%rhY&EM)FENk#cLKv738VM+SJfhd zDz#~*Ug(a8wKjM{?dRMxqu+?sCGfO~v%c`a?5Cbw?8)H?RH9#X(x!b6?fL3O-5h=d z{n!{i?$3coaXn!(*h4&e=BH;5e)xfzRHN5EJG*(?CBZ)HzF(Z3_?>Ugy1k31O^|z- zKozTcX7M#I*tD-pR8{my)g(&o@C2WvvY!sXS3`hbK^pZp_4qov+sId^MeX^uc}XoU%Dz$z~4@?I;rL!GTV@ z>oCC{EP}H{m+&ff=V5|aQhCB6)hC%oJ3N6(w6(lG!!-!6vX+Na*f2pXsXW06u-kG^ zpb~A@-TL&}s0xo%o>0DgW+)PQ6<253MCv8N z*+43GYT4}hU4mFrQAx7vox0HuUzI1yIXr<%Ik>KLtKwQKRaKon($!M7(c!AyCGfNf zdSRGAm8uG-fiA&0K`Kws3xSw(NB_=`eKGf_Y-_{pA0K#ZCD~)y!vy=Ir@MrY>k0PC zVFDF7ZuqC8Jyltbu{=^S+lSjBg4TM1_1LY-S7JoCcRprSGgnn6*ekm^)K?IwXveWz z?uo`aZ;ZL`D^$MUx>d0zq+(vlZmf?+4m@q5JQF>EN;GS;TNR$93j2y|Ms}D$C4ybM zOW^TwWtV3O>s%_wU|d+aA5_1L3KP?$!0Vt}EQp?_WRbgbOCK%@+g$k0)ev)mDx&fl4{BMO2Ib zed(^r3A=Z1c#n@8^VQEw$^_T;*p8Z57^NJkh0){Vl8+NWnc&(^wGcZ-sjnv9(l%ku1ug(4s83k+O8ZMHcLVdJU(v97fzYOgjE<+ zsU4Zvzi53mw|PZ7g2%^|Z9SdO)MWxy>h(-i>o(?dn^$4XQTezrUwk^Oc9aQD`mr4~ zu`o?J!rVpWE$hMvicNxP3 zDn-H;QSUz9HBs+A-s9tvkKLzCsCKES7NXvLyvN6l`Rd)LOsJ>% z76@BiRqsBr9Z}`u%9fIQ&N6{Yk<89J+*;nRyvN5)`SR{lCam_LO4Vkf-hI5s$CYh8 zop+xyfl86E)wX)~@g5&H=Bsy~GQmz1tF4K8_wgPdmwfC#WkR(}MYRz1?&Cc^Zp_!W zU$qI1hg;`8`&Dd5RQb5F{q9rmS8W27KF8K@ly@IgDPP{Ns?pO&Z=QVrv@6Hj$vC|# zuD7|5uB!jM|FrW({Gf+GrAVAntLis5Z+bw)&zuo+;86~U(7yEDt=h2=s(K>j@B~jF zMeuoO`RJFQxPEfQ&2L-r*}Y1(KOt`~5~vgjTRC=&?tJ3X$t`F7^u&97T-n^W4HKvo z30uT_M?d;&hgZ(n`qq#GkB=*R&3y-1CQvB{wsNeEs_H+c_HW36$Hz_f-uyl15D`=< zMm2K?{_n)e&OrT1Am5FN6#$(QSpz?8L+X&|8qGbY=a$twPTDN&M zXI|m)ab?>~te*%M2~^5~tsMK;ZGFst==3Ycip9N?RM`5Qr(acd@#BX-DMD+|dpyz1 z@e{iieon+IdI%~}B%XMys%k1?bE*oDk1GN?61zYAuKAb#VQIrxB9(6k_n*TADn-H; zv19t;Q{FiL+%+$ldyjIUlFj|@Fo8;uutmIkdiU#2nZJMGS3(XvKCW!;cZUg7%7HE7 zw&@$+^-CL%egF4D4m>`tY`)VOCQvDdDf8X-s&cH51CNiJ@`Z1+l%q^og+Z0tk%Sh>J`CTUDMy(=mBu3z)w+%OgfU0uwA2~^5~t*WX;o4XU{E*>9O zHrK>q0+n)Li&)D~guO@h{N$e;|HbED9Ijneb@$?*oU=7PPizpj*1}&?h_Ia~{Pl)x zJJp2KayJKaS1RV1Y-?Z7(^~Io6Bawqt7bchB?6Uf)_k`rJZ-{yVWwUfCQymKV%x2= z8^8II9)doR%J-E&={MVfr%iAT?pEaqRH;q%Nxw0Is`m{$&r9PR?@UF^Prhq*?Y%GW zX$PJdwCp1fDiQ?qLFzXj2xhoOq;a5~UM>CpagFwvo+O&iamC_xi+5-?4-- z(C6HFhdX-RO@upo@9~6eMmF5h>uw_4(YFay`kZ@p^;LaG?>#=QK8F@jpU8YAQqd;a zjOH+bN|CSwkx!Q1ZJ0o%NZ66!r0+dGu5A7$V3#zJv50A6K@` zl6*og6Q~pkTREyJcf#J|OkPp_m1BF(8kHj zw_ort;Vim3EfJ`c16w)vuYb?JrGCc#0>XQg1C{LQ^}n$E#UqD9jxvEtk+7AcTHk!e z)swgE{Mv^1__(q!{@z=b&%5gJkfThXQY37pSX&$&x?%FG=PraCczj&hKlaGmm-AaG zMFN#_U@J#8QjS;KD0+{NOTN>$#rJW_gesJZc0jC-j-Gz?=>mnpLmatD_i4#c+U@q9AyHPB4JyK1GkR;z}oP zatsit6bU=FBdw^A1CNg@TPy9k+qa~4ln7MHfvp_dM@DSestq~t__!(GEVW~RunL1J zwIdU&>Z=X<3YCv5+j@5&<54D1rC!g({zV(}xvg`?BY1pV*)~h|F|W!5D&@ddUsa1X zcjwGqJU*^$oB8{g?PUU$a$tuX>$V2xtU)|Ju54Si`&j2ykwB#!*dfPMIoM0^__*ZD z`&E%pg;LQDh=r-`iF5WuR6ee3^&$IinLx#OKNF`s*nk1Jan7G zPgk6cE$JGC$H$eeIjt+)Fo8-rur(e#bmi0<4DG<<JZ*xN)~(7DsC$ltVp|pIOdrY{R?ZDSLR< zm#PZmESqY(Iq-CedL}m8;fdC~Vtusc6;JMz!)8f1A<4FO)Xx(e?ZDF}Y&7#3u342Q zP>JSQX1C>dr1FIIbUuqVb9e%kXdBJKiqbginktq0P1o>fyC?LGmRp_S*?pT(4zAA4 zsyu;8w5rYDG&FMHk;)TlQ*nKSSRzo#RPAI&SBMMRYQ3XfEt&|2xw;R#ftS!uoP2yd*X z+CyB~)~5PyqA`NR@+xRf0QK|scrA*1M!NQeH4#6rrkrHTgw-5WiLUpOiFO%XLFHl8 zWHYahm7|X931(>Mt40oA398|$6j$R+g&MWvuS#u#V2%w}g^GNo@rXMPwQK)4{;Jr! zM5q_?&M??lQi-5Py6rGeSQ9C3*l~`(Dpr>W_O5OYKZ2-Yj@TnzLaT@~E^FfWtMUZv zv75tJg-SW-kuIUyd8(q8>x_H+Re3@=I8(RRU}Jp*m2%J{p{o2;?Hl)gJ-pka_n?xU z-<9H>rY50hNO*dPK#Qm>hbpVKkwb5WVe>u~ZwxgFa+HYt*4EVi5>R>ORN@KVM6(>> z9X#b&%S6f%JXGa~{H9)IgsuqHUy;N2l_TnwXM!9hLUgFg;>S}Tl|Y>LiFYpj`RH5p zTh38mquNz@!eXyq`;qpq!?X!h`rPQf%Ga%G`|`oPmpptVSEAY@aWwqaPnS6F6VXFH z2#rS`!A8s51ihf#TRzqPbr{9PLk@^?JJdfPzv?O45yn}xjq>`V|Mp~0%kiWfnP|+b zMmt(Wo@1&?x%>JmdT0lIMX&b~xgDlja$}R{ZfBO{+C5>t-t)US?bf2=^ZpUkdalPH z#ucG_{UhiJR3bD^%*2K1UG`4Vm+V~`dFb7}-#KB!k0!SIYXABXd#?1nJzpZ0`rQ+< z>4jke741+@i-_--oM>;+d5@19^VPjuCVcN=D@A_?%9nSzGGXlqs#I+z_Agpr&FL#tKCW!*-Mn9w2~^5~t?`KOm@wveeB79?-gnD{ z&84tA$9B|2e8+^ji^|6(AG5tos6uuhY&B!Y)YjmfHHgQ@jrr<5u}r9^*|o8iW4CsO z4R!`RKCWyN=Dod4po;UVz8aKwABdDM?8o+*dtM@sA3;B3v`gjtsC@m1^+r#M8)jep zyj~ym-rgoyABkRK4K@kiSAJ}cuPRS4d%7(rR;(n>KH2n0moQJbl2JRmHj%mno-R?3 zd9&r7uy|o+B)aG7OkDM%Y*onqNTlJaJmL5Da#g6v#~Q3p`ub~b{vtv4p*J0w-Sv{s zhBw3bYlvo5`nH(AXp;T!_x;!S55ICS{ANx4*Hn53>!e+m&u_YO>)gi`QOeOI$nFW* ze6Ff4Z{)z^Rm#n;lynI^o{-JwBC655tqogtT|QAP|I0Sm`kX%O5_l9BLN=dA;^!^L z`6&k;ukwA^CFoI4$mVkq+ee!%{(J7+8rtDU4O=-F*)D-caUo>$c_dD;9KV}#;PEQo zhh4(=kEi)u#KQFVE&ho%w-k$!QVwi=?q_0?z~c$od>(VW#&VpKa^UeQ<)9b31RhVw z=5rCvHAriH?d=@(I#(<|OZ?1lj`_aB-`)3$m)5Yp&`sOf8@OVNtzhF2_pi&NO5o?QEKmPjp$F6%($brYlm2JOA7=9UXm_Vf* z*nwC-`=vL{Hy{4)viJD7viWtvVFHyRVT)LuUi9m4oB#bE-@oBKKCWzjwQ!g~rAXK! zu9?2+!87N-^_P<{+wu6gvia4*VFHzMV2jv2efufz*tq-l>q8DaKCW#2oh`qHSSC;@ zhvpl^s&cH51CNiJ^5x$hEE85?P^ETcVr|{}YC>P3@^NKbAJ)G$StL-U@yJBAXk$KM z%u)HcvTc^+-^?r%sFVX+<6$R#Td~YtJU*^$ey?+wK&2emBGyK>1}CgRJU*^$TbtpR zT!#r%%7Jaf>e|Tmk`?w6JU*^$+hh2>*8u{Ra$v`*Y=>Lc4(B~Su58UW?R-^r#p-*O zY>rOf@q?Q?1gaFN=l5P?thJZ9T{P5Aj{Gh_K83dl<&a9Bv%7|NGzc5>@Vl;3+1P}A z@K`zOxSp_H@A(y4A4@hW+2yKO+fw;mf;M&Afu~Kdj|~&3MAIW(!mkg%9?9J$@F=b) z*fE9)RHEsTF2Nj?%FldqcL_Xgg0(qJpb|}wh}f-P*wF7rg?HR--G?`W^c>&61%n-O zjP%6of9?PDMn!lV$P==83uc%=rAXK!s?n2H(KA+=_xQLmU;TDXnegw*V22!wDaS@^ zM~vm;l8?7*sv^NVI8+OSQwPHab1`M9z* z6M0*OdeK1EB(m^X6}%d1kpFvq$%tgk{ZN~H=_JM^(f Q&=Y=dFIR<%d~pr_A59&cvH$=8 literal 0 HcmV?d00001 diff --git a/src/arm_assembly/meshes/wirstBase_link.STL b/src/arm_assembly/meshes/wirstBase_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..0e2de13505b41fccf08796b9ec470ce260105d92 GIT binary patch literal 12484 zcmb`NPsnan6~+&wK?D&96&4ZEeF++dbm{&6I4Pt_1`Vtx5)DF>Afl9lzVZ$n1Uaxt zh&9kxoJ2^F^?u)dCwI`G;8GwGcDn=@p~MU)LIYjT+WT4iS!eHaY>*(V@2vG(&tCig zIq#jO|L=cqdb+d2G~K*3O@Gb% zkY(bzJ4zHYec^-O9uh5T^XcDgKm7A=RgLRMOGNP??~K?!@Yk_MBdjKm0{!RLetYzh zuRbzp6+N}>K|t??JHI<1Q0=>Z@UXZ2cVGO>(Wh?x$n9l<9%v}>+5i6ZXnN>}0}Uk@ zwO&UawKkg8+Wpc97Ei4_#^3tw9CKkb=85nq(AL^LMzbgAsTx5*V?5?_0#TLTFOIo; zs6oO;_Iy0%rXB>NSjTwG(JFWlrG$;m`Qiu$!Kl*0d-s?Nt%~)bgxTA%hOQ_^)fW2da*b2l zub#2}YWL9(JmCA)WB+>p7B**oeSVJTL&7~hA?stV5d>EeHDd10h_W*v%4Ur3AF#QC zN3AOAmGD*cP8wz942ZTqX02)jXFh1oc#p<8mzEJMdg;M?YoJfXHHlP zx3SI_2}acxw(}zq7UiBA<$0+W`_&?W5kwR<7U$IwXJRJgdS%6#hyt|&w)!PT`+Fp8~;y(HCv_QHcGCCqMn$r|BNwT`wYCIagYdt%iHf_vAn zjvyEXnmc$;(1R!?xCgHji~`Md17g~h8`C}9ul&3#J7-#tv0wEBW{DECc{;BXh_Y54 z+bS4qgh%t5?azlAN<3N7TOZ zDADs<%{vpduPzof!oya9Fx#I;Uumn9h$zew?n+&&!lQXja}fFIP?vqLkp?)oUohDA4Tnp2)MW zyg`7l`q~@hY?j3PpwFecqF~3W&1247C2zdrpmIN{`&Q92Cv**FjXA-2Y{cMRQuDfp z8YHZ*26vx1!6-b3c7MN84{RlDY}y-S>Rsjn!6?}1!+8Hl1U+-YX3XFguDM_{3O3h@ zkH>-E8L+q01DizI^QBdZV5{Z?>+3bJFV*{1?1@FAogeL6hI(Lg9+&4bcW13CxipHI zVi!$>R?G1Kv0Mi|h*E;%-&d{#qiU<#+)b_GtVYy~@N>7ZCzWZAlPZ z&Ao=s5*`1v?@1-J;wxC(y$2EK!0{)WB-) zTLpsaJQMOppTnaydBTNr7W~>o}Mj;x1N98teFWdY*JJibe@+Rc(BJ4N56}=dQdhDKnp61&N?6~!< z5+21~A8C{sF1Jb7n5LI}w)cd3%B?1~s?xSIL>)6C>ZqgXJ>uhG#NFOk>e0%z4?Duw zHriXqfIW?IO06my+IA%@+IOG6awRmXKKGu^tx}H?Rd1^rrB&+Tyh4;_TGJ@=wdC~? zOd4e-=3Fhxo_F&q`bs>FsN*!&2tuQ*KI~UP+<)otjRz0E`tdbW9#FT>w_N63)|L;zbC8{jDiMj5Eq-jKYHcrtDpa#&nv#s zAusJhw70uVU_Ik*s6=U1BktSWcKcn2f82JLYbZe*8rc1K-^~O)nin>oqmHwut%ifu z;(G}_8dWQAZ+D5XK8Y_AO7l51zPWqFG;Y6g<*Mc?ttvF@;FpO^AaDI;6l0L|NL<_1 zzB*_w)`6&^-M#^20zF!P8IAYl(!xgE+TwTfy-@mY|KwN z=JaS(siVCQX2QnV-xQT7dX0E^gZ0O`s|0P0OFa_Tum;grz6O<`&GF#B zfXD=Tlb#bX)S4kYH4)W8p^Ty<7M8L}Q$I8eG+TVj>s?4gGf+nV^T` zK|MZ49XIs+h+H5TRqODRs3)u?JfniBdO@U94Rr)TyN*YEd(DLPD9_qN^Eq0TPG|IJ z5VT7j3pW7E8+V38^EouqEh7<(Vja9$W&$m#chN-iIV93;$y-8#Q5+BcN|p)qXuXRj zn$Mwe(0iiyC<#VE1LM!%CNe<}$C)&DqG|f4-E&{H`|dP7zrmWr*GzmPp^dL~R*!$V zE8k9(P|t`c-}>ILJ1G92rjKoupp7W2$90N^66zTdMFTaUWz+O)`#RTh(JoQ#`^+>c zQNP3ax0g(`FP=`E6pbhCdz|KKQAYb$wo->_1d%;G5q))9`s(GK60{Lzy&zwTEIJ7F zjEJIPHJtjG-+ybT1Z_lNop3JrED1tABO+S(!WOC9@|SI}^x=5^1AC>pi$ zQ=3<;!wJ%*j`ABq%q#Sj6SaR9iK04NQi67g^0`p!P(r`&W}^KT5;M^>p0IDc znyW<_?c*Nv3bWmb?CFW&P|t`c8dw`Xf|qY`owIj6 WFQRNTV~>g5=VUTMJ=9a8XuJ)<^e)~2 literal 0 HcmV?d00001 diff --git a/src/arm_assembly/meshes/wrist_link.STL b/src/arm_assembly/meshes/wrist_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..0e6b6ab511e886d43f894b64b5e3ff6133b1c466 GIT binary patch literal 50884 zcmb__4Xm|kb=6HNkQi#Aqomei0cSp_MW% z^(7<62lg39lnlMrTSaf7W{fk==2FA4^s`X3(W*UG$qK_BqR`tt=A-)INQX71jJx(k z^S{N~?S5c%3#Itg`%Ol9lOesc!wjRM=cPGqm3l6XhdEZZzp7S^49(4oM)uMuk1T(# z_G4#OGGH?YuVxfV5tzZn`0}-*qo2g<#^pm#{KVOfge+U0RTeV=6$LRQ3 zt-X>_s2{ofnX}*h(pzeMO&RG?>|%WWowuAl1CH z8HrNQR~YGe&h-b5U;nB*-SdvL-KrUb`|Z7l`vGsG)br9=)o=@%A-&^GjbbEfKIb=1 zaXR0J)A<_Jwd?QIlFGYztELQj7mP`sPc)KYRJ=Fr3=>A8VDrSW9)GSd_^w{N8Kd?` zj_#r#%>dK8C`W`R&WeUTK<{Fw-dX#w#*}dvgL_5*5&OIvzphZ#nZq5hS| zmv7>9#_PtZ)D^#XxLxdzHC7XbcQubqv19L=G14>BbA+oIg^WFM#)wmfNVbTtU7RH| zMtYJ#mf0NLvF9^Jqv9l*X{0CPZjWyEBTi>)RiCD<%C;vXZU(bfB_mPpHCwu>!B*^& zA)7a~86!Pvm9we(uw>w!_9R0iQta(kC5o~AS}@v^49)DiMl%vMUy0Af$;{jSyZG!( z#$GSm{#oy{RTGBM{Ar|r)_cXkxx=&YU3CKxk|@83EHvL+Fxs=h*pu<>`~KoL|Gm@y z^za|Q{(nC8-w=1P;~evS?{;L|b@P+|`iD-x@|CX?!yam+?cMb-!zgOlCgX!Q&)#EHi@FFZ{THH=c9`ZU;P09mM^e}hepM{d6GY4>0M`M3X#(l9FH()Q_dm|+w(Y?CqW zCF#kyvUziwWz-6zP@?En_(0+Y_lH^b3QITANJs9qiADp z`ZIuShEde8O^u)8y1MkbvM1xx#u}?n5DqhpqK0iU9)A<7{nD%5o{US|x3j|xqlU5K zeeOox0G@rDG%nJUaYL`p^KFJ&cozLA#$DGmR;6K7#-*(>tUTXl7)1@+?D+?;Y0fXx zlW}R|#dTF0+YFD1bhO>)|?NiN+(Bthk zK2KD8YnP$@&;RQ~`;WcjEj!v6k>26P&-&6cXZ+2l-oM9dx72VfGK`{)mNBDN7{iWp z<5r^H^r_G9KmT37=+n9P#A&OvGZZQrf8pOgy8rQ?I9bt1hEdt4Wk1BIJxUs7yR}P( zJ<*S1yz!s?POaO+45Na*GK1a$h(b^I>rSZmd=laXN*ZO!Zo z!#e{}JTKT6+HUPq!=5<&c{fjB!NiE^gWcCIWL*;}pM>c@7^?P1(x zD7#krkqo2MLaz2Y2d6WbD=`^b$157iFp3&p$IG5$mDIjV6f?-{$}_uU*fVD^dpBpW zPZ^`g_KI0*W#HkS;0h)M>=t=95khEZf=p8CX@&WAljC4>7R*T7OE8Ag%q zQC~9bAu1W%vAG5gGmIkJv$7yAa~PVEB|1&$Pqugo#RqQNdoxLCs)|kLJ8=w{|&J_AqWTv?{OYxlz%o z_0xbERV~pxdj^@!D;io?3x-zA6%3=;Dv!vr=k^el46Wl8jbs>=|xwwAojh(8kn#4WEe#@-mg(#w#pu&lA+yhrRT{oifqs3(a~Rh`|k3VmpFeo925qmgk+EB0 z7{y2*d*jnj{?eD;D2*E*{KnIZ>pxo>_GDbzUipK@7Q-lN{G%Ux^2uku?iZ!;nO}YU z>94>4)1_fg#-;5QGiZ?U*%gLS)cD2^{q~Npfk8A`>B+dXy^aqvjAA7Afh`^w94GcZ zJsFqwbk4UJMlllSf+Ig@ya2Puc~4KqrR_B~FgV|945Ju{Yl>@YV0=ARAy;F1GA?be z{G+2+VkL6j{=OB4QH+GlyLVXEWI*HG40|%J^mtFy3_|1Ftx5*^Q7E-`KlTH;ap9h$ zhvmt*w7tjlSgkUQq6X$;Kh6hc@WOL$PsXK<6|?8+A2hZYMo|OnYCo^9u00u- zHdcO}^MeLf`zpgIY9KfEV}2n1r8CN&j7uAR*mLy{8e0sbsDVD5k9m%9y5x#aPsXK< zooLV1KWJ<*jAA6r#re2jVZJY!Bk9Sww6Uh@`E$@9!+WA*C5n--rq0KG7psu#w{~iK zl0lnKM_8r!RE?`0E4^|gV{u`#)6^Vo`p7Art{;HIg*}? zOIsry42;!+VH6{248_n47Q>#5OIs`E3WibC(7F_kvYRCzOO70 zLm73Mo{URd8GHr9C`M8?W8OI*+8Ij2o{URdJCQWD7)DV;yOK1J8{FYa!=8*wTRW;W zINxgwqo|?XRT^Bk%#rkDT-rDpj)ol3`N7j*!7z%EVBb3Ah|Uk5IK{9h<4R9YsT^IN zQEpW-bPn#-TAqXI2k%zyxlYLQj7yt)6<7Z-gF0*7kBXHjYH+V&CiI;1&QJ_{l0ln$ zE?56yhEdeup358=7`$6CN79pVX=CMAjtmT*1ZxbV7>Q>Gb7Wuu%^O)|lsy@jHu_LG za+qNhHFyRx69xvxiFZ+FZFw>-ZOldG$YF+2)WBR+jtmUU_bGEEJsFp_Zsgi`$?%@& zSczgJ-bt7vg9dLg-rMa-25mpF_W5z@y*(L5QNzy+Bg1EuJsCIj@XYzrCp`D`KR)Am zm$yG~kDouVc5dAI`@iS*li&O&-*o=Aul)A0`k`}>jQ{d&f9&$l{NT&3rzaV-KlbjY zoWJ3_e{_f6y@K&;V7zRFVH6|jln~>uz47Un5B&7|O2eLvOZ#hn@8ixNdL}=oei<}g z4aWT|45O$q-b;P}j9>b|lP+)hjn9^bJsFqwBme!^_IRmP4}$RrRv1Q6L+6JWPyLP0 zUOe=HC!MAz%wlQSlW}QdwVYwqfWcMrQZPQV!Z3;&IzPntz6b6( z{}0c2UTN5qacLhNedOLd-~BZF{61)W2N=J)!Z3;&er7m2`d+pQEB%vV*pqRkcRcok zxq%$1RwaYEL6rKzJzxD`ZXie8kMd+(+RTk{KA0OGU7a{5iPC)Nd5|;6+(3?aKFX5} z+RTk%U2&B#H;^L>hEdee`GJ0LB{DaVBgL>MF<|KYV2&&pMp0wDm+1UpjugY5j7wW9U#((pAV(Gqqp0Cq z)X;OTbmYiodNM9;W$<~QAJ>s13x-jQq-+L5=g0NCaeAgFiC1%}R#>&TG>!zf1b zt#)8AH+Tz3PsXLK9TgfnKdvK377U{pNqeOj%njs7G3?2>wD|;p)enZwj}zp`f?*Uj zcpn;`KXiVaBS(s1PsWuV&q1|{xxqVD#!8g>u}4OW!Q9{-D?Q1ejgjVg4hH9gx#7{( z$$6eAM$)M*26KaVtn?&-GVL&W{V;vC@-qX=CN{ex>|i zZt#vZXBfrzb-xmWxxpJ#dNM9;_JOVJ^JA4^6eH<22!_s&OWv{4lW}QtE;uK{d@wh7 z$C@*Y;``W@&N(LogR{pwR(dinZLTS-e(k%=4c@Wl45Roy*A&-7pC6aJW2GnK(jIby zpI_?;|ClD4vTuE8?!Bp{OH%N zXe7fZvYn4h4SR@6#*;qutn(M$|L?Kd=lw{AQDi$GmkfJ|O2)-+e&_kef8w4Mjbs=_ zw)1hRVGmKs_+P(%=lR9|e)5V&GK?bI`MA`uhp1#+-1*&@i{W2?PIE9BMnykXvR17+ zu6`(P=fXrJ=R3!muaf($@T6!7z#pt?3npJsFp_ z*73=5tde0A8OqHShCLaVw({``hEZf_k6B^ZlW}QlU%7%|6dBqpR~U)Xo>))C+YEac zHyJt&Ry2}f6xqlPea|D6+BM^f|*^!yclNfiqLP7}Kf#H!lIO`Vifrs-H783Adx%N~_O270I4c^- zFp6yKu62`IYS=?mGO!1q;KW(cNQO~lKXJ|4dBd3XHS>D!+6*6ZnET%%|^gAX$r zMeW#2o`3Uo-}BE!FcfIo&W9$r@!)^KVKU5 zWL(-l;i$3AXcRQs2IJA2H-GDa>;LdWf4wy9$+)zAQd47_(I{xN4aOI5e)8G7>rcP; z@0Nx=8JBk6WVRWNf=1iW7{5QrIkzX{%I0(Q3Hx!7F`$M>!9YJQ+z)7!C*#sa|M+|^ zjV(r_hOuhKW1No*&xcVNmp0~!&*!DF&1e)f+D6YG;kvr?y0RzZ(#Cq@^Lc4(Ga3br zwxRJL^W)O_VNb@TjlALWd1-7j8U>BEq48ek`K9yRo{UQy`OoL`9S8>)je|=aBFO6+RqoC0?G@gGG`|hRpU3)SvZS28(J}-@JMx&t7HZ&f%52wMU zPXl{0Zt2DI`8LDP=ghRh9&Thue^?sl>B+dES9gYOhFZ91q_)wIhi}tZU8E=DmR@{b zvdz#)mqy#1k87Iq(y%Av($-q2erz)ug?_XRjkjLY+P!2y?8&&awWiNOIKXHWBDD?1 z`>!bnOT(UwOIx{F8rzIUL8EPGJa|oeNom-VacOIhDUEGLqoC0?G#iw`Do!_GDby$NmiYAY(uck%FPK zw)z2$@?>1v=pUcxs~=xx&v`m87>ydnsu_=QK6q#FSd}N^(#AaTiM}+p8I6KQ+Zd}y zxUP8jDGhrvE^VwgKGB!PHltC{Xd4<2GCz2SD-C-xE^XuupXf_to6#s}v<;2-GS7Jz zEe(4zE^XvLpXh5Z*=95f8f`=4P28_|r>_0Vo{UQy`xu|-OJkeSC}^||jptv}vw`=- zij{F`W2fd5z4pW{Mx&t7HZ=atZF*v<^TVEuTYB+Czs>OTIWw(oXx!L*Dl#hcb|6Hh zDaIPU=Y{hQw!axj+pS$P+LJBGNC!3Cs?fNiRnn`cJAY$r*(!ZMs?I*Im`R@&t;*ge zBUi~-k8Bc)J@@K6I9C2H8QI!>>gm|rt*h4;Hp3k}TR@EvfVFdxHjDq(kP7mW7I z7?_*!cd3(BC8JSbcb~R2YQ3sYWs9-0N-|S5AKn=@vbOUXElGyPYDFU%jRLz{xadcF zfJ%nuW^es`W~;qpB?`7OcqOAU zCp0(JDvaSqezXU1lYw#To8=;Fhge`V3O2@Y7?DLk+B0Kdr28&9)ksF8z{X1Ko8_Ew z7ssP+0N8_v-Y z4z7^lcPx{2m5fG>dMo|tdu0j(uju-46VK500R>yRSsL36 zqZr9HHO6Xh6Hms4tt=>wZH7^dWSbgeeYlAy^@ z2N^~&l5J{?_2DL-j0?MWjDrlL7|Av@#%ga9PsSbf>i4zV47L3%T3d`)bbYvqC*#7_ z=#FKGhBcr(5OJkd16eHQD#`tN} z#FKFc``8)>8HG}$W{lO|CZ3E78$ItEz&67uMzYO*jMd&Io{S3{GuZd5ZH7^dWSbge zeYlAyQJkO z^-6ZGk|_hDSkG!g@gB5K?Zd4?+-9IvJfkYlryAK0qtvG}XYG`sxI3cM!n(;g(&1Jm z19QU`T`}iIgyv{jn5YL#ZQ?60m>$ErVnB~O z+|{ir6!knJY}KT42{Q!PCiS9WLAK}MkzsTt$xoSuwJ8?Vmu zZH7^dWSjjMZyD*yxU?~YwXYmz7{y4o$(U5rZBNFftzDw>e4AkuHEdJk+p~^vdonI< z?JFc4U>G%w2=}YuuUb7W>me)FjrzMNW`tgyINJ=hoi*s(+G33N_Vi?2+8W*38MYZl zF_LZee0-WnPsXK<7uQv3Y%`2vB-_*&pNi6xacLu?xY|o&n_(0q*`~($beEotEBn|Q z2N{J@q-Kmysp-kMwDGDvn627o7{y4o$@pN_Z*Nb=rHvV^v*a+tC~DXyV^W8{JsFoa zR%L%OvCS}w8n&r1-V@W4acK`H(U4Kw45Nmz;^!#Cf7_>;8KF13GccBG6RCxD*OJEG zc|P7UQeBL?tJ5B9a^yXc9<9;+z#evI+OCnqo=i2eRjzDAhIWaSe%M1)GF-E0sgVq$ z$kxt)-p^ZQ4^hc*&7#8$qsYdb^cgknxjjTB!!?VR8p$wPIu6ry4@bu^Jghsa4$JRx~`fM7atSZD-nYKJ1w@ zh#cl;-l}96C5;-9RgE62WVpW5VTMuE@JKHi-ergyYVRB!(H?d?;^+OaXU8Ahq+Jg-(}Fn73Q=ww*YNQP1BxhfUT*OhjL z>Mh2F)!55c-mGZY!??*%24BH2ifr$8%lXjmQ!P;zxmA6Ko37pLM>3R;>#eF-Y>83X zr)8@$i?pt)Ra$TBGg!ICxXI8uUTKwMkzo{V<;@Dio;gGLcm=~KvOVg{p4&rIGPGl? zXe7fZvOPCd!R(Y3%v6u;T-C*DA9SYTTmR>(OtT*ttdgc$lkH(#GiYn1gQ2==6NXXL za7C9vBkIGoC*#uA+(=`KVH7o7(PhwJojcWjjkxtFp1G z@3kl6()OM|v?}WLO&CT|W32T;4c1{*-NE)`T-us*^<#@+6g9?LFVs-2Vpj2JPsXLK z43% zg9aJi6CEp2)EH~MP=l*bwI4BEPZ-&d6*Pr1^)w>ws%s4>)fISPhqO|~Z)p@*~9 ztzsT#tVBgWc4N<3k2O8XpskS(2KSRW!zjM5F%&~HSPXkIE^V!tD;P#mL+eTmu0+;j zO;5(9t(6}P?(cJkQG8$dAqH~;I;ZK$xU`kQS1^oXBxN&x@5UKqZeYAh!=8*wTlFgJLcNl!9p ziA}X8J656?NvDJus#RPX_9TNgDm|Q|N&*q@nXi=p#_we&hJZC4N(R*7mAFBnEqL#G5Zm>;TD zTnu|ME^XH}8Cs=U#S4Z})X*uRJXfva$|!p>E^SwI8Cs=U#S4Z})X@2%o#F1GRHS>@%%uo$xGELK1ol;4LzKLIzL#ec)?IVSnJ;X*bNNU z@n(*sC*#uANQ1%E&e3I#%o#@UeT}*ps#RPJdonI$ zX=^6}gY&^U-prAVl_uM7Yw5`hWwOD4Am+w26r)fGA?bb7~V_3;3{DqZ`Hc@-JlpoQG+`x z*L9yCtW{hZ_GDbzx;ZI7RI7NwFp3(wUx}ew#g$R^WL(NhVoAt_7Ihf{zlzGBN;}K?Yq{pRrU~-jQ&R5LL(VQk*z&=r62YXm5lXo z>SZk2M$z_NYuOKb=8W}kPG&6HM$y*ExnFA7GiTtNXk7zhI)lkDifo7)7@4TFbGrhp1!>RS{H2c#_i8`(Ug z)GF2yK2L`9urnND7)7?maD`z{^n+D_J#JSpG`hvmT3BJ&lW}Ql{;yydMTW9qg<(&| zrLC2ZQ*Az0$uNoxW%CNdo{URd8GHr9C^EFitT61!xU{veT){Ak4DFRGj6`Wq^c|(D z>@LU3u^2ZQ+O=0Sl3^6th<%?kVVdXm5S0wrXReHQkWnc0Tz9znoTF9ckw!Dnr@brV zO*N8X6xo<3ekQZhb9;zN21dOr<4rY^VHDX|Z@R32e8@YiTng8q0^w3U*zVD9TLC#y{Skg<(&|rLC3Eoo;qRPKHrrC^uIa_GDbz%Ev1hMv#eZAPP@(Ka+DRnpp%acN^6_q~0a(I{xN4UI{)xAtUQ+E~Y3gKnGA zC}^||jY-wI_GDbzSjS!QWSh|_XtWKDN%g(m{e(QPsT01xFK&dGzX>8Hd-~QHtqTFXA5=nriNBYwQ8HuDD0c>jcrDw zpwTuoCe_&6lW}QlM=gzQMx&t7HZ;D>dit(I-=2(1TX(I}*k&{e8f}*bPMn){`wLIT zmCZYxG!8HZ)DWo|U(CM?K=txuT-xY4&-2pQW;ALTt7c68T0zIkxU?~YyqA>5HltC{ zXd7cS`Kt@<$+)z!5_!KWjcrDwpwTuoCVxGmJsFoaGK%-a(%5D+3L0%gZ?aBKvV)9{1yr7cc}9(Y>2IPAMx$VNYZv{H##yv#PrF;YV6R6 zTTutt<7M^Rg)UAW?xq?Y3vnAY^n94jEsa`z(qL6!uice( zm1{Q{8ikYPDoIA8pwTOT(JIYawM27XwpPzdtJ;IO$eatO+GeQjtg`9W7GtdTHt}R! z*c#m(TeZzFiji!SF;;tTqaXLZ{l_n#_4R-I;^^pW-@`Zvp{lBF@1Ar0f#cV|>ds^MXN>d& zI~lv9qr2aIdim;)ek*42_q-<=Veh_h_iK+|cl~|RNJe@#7|{6mJ-=}AT|e}CXgqQ= z8Da0pK|&Q_x}JiK5}a^!ruMJ<Yf_E?=w z`cVwEy*$BA#yB77%h9uv5w_=C8k&z{Xb#G=!GMNV2^jz4S~9}++LcB!w3f=V!GMNV zJ2W1=Eg4}u2c?k=AGLgVSq!D0m4JA*Wmq1-;ifQI%f%-~0ECL`<}=DPQ> LDI+}_3~2mc!n>=> literal 0 HcmV?d00001 diff --git a/src/arm_assembly/package.xml b/src/arm_assembly/package.xml new file mode 100644 index 00000000..b357a057 --- /dev/null +++ b/src/arm_assembly/package.xml @@ -0,0 +1,21 @@ + + arm_assembly + 1.0.0 + +

URDF Description package for arm_assembly

+

This package contains configuration data, 3D models and launch files +for arm_assembly robot

+ + TODO + + BSD + catkin + roslaunch + robot_state_publisher + rviz + joint_state_publisher_gui + gazebo + + + + \ No newline at end of file diff --git a/src/arm_assembly/urdf/arm_assembly.csv b/src/arm_assembly/urdf/arm_assembly.csv new file mode 100644 index 00000000..791664f0 --- /dev/null +++ b/src/arm_assembly/urdf/arm_assembly.csv @@ -0,0 +1,8 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,1.03582841131364E-18,0.015606490089144,1.93574980271696E-18,0,0,0,0.93972387307473,0.00251429022255044,8.65861551705636E-21,-7.37485241366082E-20,0.0044229329573873,2.27711298016934E-20,0.0021336882069818,0,0,0,0,0,0,package://arm_assembly/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/base_link.STL,,base-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +shoulderBase_link,-0.0122586821821901,0.128577249577976,-0.00370544350042927,0,0,0,0.996366805002727,0.00652370094208947,0.0010646616036529,0.000283127005277364,0.0050231810993567,6.82113719161566E-05,0.0086334538259584,0,0,0,0,0,0,package://arm_assembly/meshes/shoulderBase_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/shoulderBase_link.STL,,Shoulder-1,Origin_turntable,Axis_turntable,turntable,continuous,0,0,0.035865,1.5708,0,-0.25129,base_link,0,1,0,,,,,,,,,,,, +upperArm_link,0.0143973999054565,-3.20061625687929E-05,0.279348384794928,0,0,0,0.985210343915811,0.0237982224738339,-8.87977733299251E-07,-0.00388532856230119,0.0293594722597602,-7.12944369971275E-06,0.00625034839320104,0,0,0,0,0,0,package://arm_assembly/meshes/upperArm_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/upperArm_link.STL,,UpperArm-1,Origin_shoulder,Axis_shoulder,shoulder,continuous,0,0.18098,0.043071,-1.1021,0,0,shoulderBase_link,1,0,0,,,,,,,,,,,, +forarm_link,1.84574577843932E-15,0.0550154986611603,0.120152803034247,0,0,0,0.517796101730459,0.00404850874710771,2.11419423634673E-18,-1.54871504075976E-17,0.00375548329392274,-0.000377385363706529,0.000788327838226562,0,0,0,0,0,0,package://arm_assembly/meshes/forarm_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/forarm_link.STL,,Forarm-1,Origin_elbow,Axis_elbow,elbow,continuous,0,0,0.46634,2.4936,0,0,upperArm_link,1,0,0,,,,,,,,,,,, +wirstBase_link,-1.94289029309402E-16,-1.66533453693773E-15,0.0987034845564234,0,0,0,0.301763082175337,0.00210344289702837,-2.71050543121376E-19,-2.24971950790742E-18,0.00305016250706558,1.85669622038143E-18,0.00107609106501786,0,0,0,0,0,0,package://arm_assembly/meshes/wirstBase_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/wirstBase_link.STL,,WristStructure-1,Origin_forearm_roll,Axis_forearm_roll,forearm_roll,continuous,0,0.04445,0.2285,0,0,0,forarm_link,0,0,1,,,,,,,,,,,, +wrist_link,4.30211422042248E-16,-2.77555756156289E-17,0,0,0,0,0.447707117943176,0.000367702266904795,1.79994501291539E-19,-7.45388993583784E-19,0.000843427086265753,-1.35525271560688E-20,0.00084474612356066,0,0,0,0,0,0,package://arm_assembly/meshes/wrist_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/wrist_link.STL,,Wrist-1,Origin_wrist_pitch,Axis_wrist_pitch,wrist_pitch,continuous,0,0,0.23716,-1.333,0,0,wirstBase_link,1,0,0,,,,,,,,,,,, +manipulator_link,-0.000730323740460503,-0.000899365669252,0.0490560099953423,0,0,0,0.397420225554811,0.00159684305263878,-3.07243227891656E-06,2.53235579936151E-05,0.0016343224414688,5.00292290874635E-05,0.000256322563994287,0,0,0,0,0,0,package://arm_assembly/meshes/manipulator_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arm_assembly/meshes/manipulator_link.STL,,Manipulator-1,Origin_wrist_roll,Axis_wrist_roll,wrist_roll,continuous,0,0,0,0,0,0.096862,wrist_link,0,0,1,,,,,,,,,,,, diff --git a/src/arm_assembly/urdf/arm_assembly.urdf b/src/arm_assembly/urdf/arm_assembly.urdf new file mode 100644 index 00000000..913a1bb4 --- /dev/null +++ b/src/arm_assembly/urdf/arm_assembly.urdf @@ -0,0 +1,282 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + / + + + \ No newline at end of file diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index b2074ede..d3f0ce92 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -125,6 +125,9 @@ include_directories( ## Declare a C++ library add_library( ${PROJECT_NAME} src/ArmMotor.cpp + src/AbstractJoint.cpp + src/DifferentialJoint.cpp + src/SimpleJoint.cpp ) ## Add cmake target dependencies of the library @@ -139,9 +142,6 @@ include_directories( add_executable(ArmControlSystem src/ArmControlSystem.cpp) target_link_libraries(ArmControlSystem ${catkin_LIBRARIES} ${PROJECT_NAME}) -add_executable(SimpleACS src/SimpleACS.cpp) -target_link_libraries(SimpleACS ${catkin_LIBRARIES} ${PROJECT_NAME}) - ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use diff --git a/src/wr_control_drive_arm/config/encoder_parameters.yaml b/src/wr_control_drive_arm/config/encoder_parameters.yaml new file mode 100644 index 00000000..9421827f --- /dev/null +++ b/src/wr_control_drive_arm/config/encoder_parameters.yaml @@ -0,0 +1,19 @@ +encoder_parameters: +# elbow + - counts_per_rotation: -2048 + offset: 736 +# forearm_roll + - counts_per_rotation: 2048 + offset: 0 +# shoulder + - counts_per_rotation: -6144 + offset: 460 +# turntable + - counts_per_rotation: 2048 + offset: 1111 +# wrist_pitch + - counts_per_rotation: 2048 + offset: 350 +# wrist_roll + - counts_per_rotation: 2048 + offset: 1023 diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch index fe7dcb12..12c7e1a3 100644 --- a/src/wr_control_drive_arm/launch/std.launch +++ b/src/wr_control_drive_arm/launch/std.launch @@ -3,6 +3,8 @@ - + + + - \ No newline at end of file + diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp new file mode 100644 index 00000000..f1e11dad --- /dev/null +++ b/src/wr_control_drive_arm/src/AbstractJoint.cpp @@ -0,0 +1,39 @@ +/** + * @file AbstractJoint.cpp + * @author Nichols Underwood + * @brief ablskjlfkejfs + * @date 2021-10-25 + */ +#include "AbstractJoint.hpp" + +AbstractJoint::AbstractJoint(ros::NodeHandle* n, int numMotors){ + this->n = n; + this->numMotors = numMotors; + + jointPositions.reserve(numMotors); + jointVelocites.reserve(numMotors); + +} + +int AbstractJoint::getDegreesOfFreedom(){ + return this->numMotors; +} + +ArmMotor* AbstractJoint::getMotor(int motorIndex){ + return this->motors[motorIndex]; +} + +void AbstractJoint::configSetpoint(int degreeIndex, double position, double velocity){ + + this->jointPositions[degreeIndex] = position; + this->jointVelocites[degreeIndex] = velocity; +} + + +bool AbstractJoint::exectute(){ + vector motorPositions = this->getMotorPositions(jointPositions); + for(int i = 0; i < this->numMotors; i++){ + if (!this->motors[i]->runToTarget(motorPositions[i], 0)) return false; + } + return true; +} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp new file mode 100644 index 00000000..978345e9 --- /dev/null +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -0,0 +1,43 @@ +#ifndef ABSTRACT_JOINT_GUARD +#define ABSTRACT_JOINT_GUARD + +#include "ArmMotor.hpp" +using std::vector; + +class AbstractJoint { + + + protected: + ros::NodeHandle* n; + unsigned int numMotors; + vector motors; + + vector jointPositions; + vector jointVelocites; + + vector jointTopicsNames; + vector motorTopicsNames; + vector newVelocitiesVector; + + public: + + AbstractJoint(ros::NodeHandle* n, int numMotors); + ~AbstractJoint(){}; + + // never used, need to be defined for compiler v-table + virtual void getMotorPositions(vector &jointPositions, vector &target) = 0; + virtual void getMotorVelocities(vector &joinVelocities, vector &target) = 0; + virtual void getJointPositions(vector &motorPositions, vector &target) = 0; + + int getDegreesOfFreedom(); + + ArmMotor* getMotor(int motorIndex); + + void configSetpoint(int degreeIndex, double position, double velocity); + + bool exectute(); + + // virtual void configVelocityHandshake(std::string, std::string) = 0; +}; + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 57f43519..d399f3cc 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -4,19 +4,34 @@ * @brief The exeutable file to run the Arm Control Action Server * @date 2021-04-05 */ +#include "XmlRpcValue.h" #include "ros/ros.h" #include #include #include +#include +#include #include #include #include -#include "ArmMotor.hpp" +#include +#include "SimpleJoint.hpp" +#include "DifferentialJoint.hpp" +#include "ros/console.h" + +using XmlRpc::XmlRpcValue; /** * @brief Defines space for all ArmMotor references */ -ArmMotor *motors[7]; +const int numMotors = 6; +ArmMotor *motors[numMotors]; + +/** + * @brief Defines space for all Joint references + */ +const int numJoints = 5; +AbstractJoint *joints[numJoints]; /** * @brief The Joint State Publisher for MoveIt */ @@ -25,6 +40,18 @@ ros::Publisher jointStatePublisher; * @brief Simplify the SimpleActionServer reference name */ typedef actionlib::SimpleActionServer Server; +/** + * @brief The service server for enabling IK + */ +ros::ServiceServer enableServiceServer; +/** + * @brief The status of IK program + */ +std::atomic_bool IKEnabled{true}; +/** + * @brief The service client for disabling IK + */ +ros::ServiceClient enableServiceClient; /** * @brief converts a roll and pitch to joint motor positions @@ -52,19 +79,16 @@ std::vector convertJointSpacetoEncoderSpace(double roll, double pitch){ * @param motor a pointer to the motor * @return if the motor has reached its target */ -bool configMotorTarget(std::vector* names, std::vector* positions, double target, float velocity, ArmMotor* motor){ +bool configJointSetpoint(AbstractJoint* joint, int degreeIndex, double target, float velocity){ // Each motor should run to its respective target position at a fixed speed // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)runToTarget(target, 0); - // Push the current motor name and position data to the Joint State data tracking list - names->push_back(motor->getMotorName()); - positions->push_back(motor->getRads()); + std::cout << "config joint setup: " << degreeIndex << " " << target << std::endl; + joint->configSetpoint(degreeIndex, target, 0); // The position has only finished if every motor is STOPped - return motor->getMotorState() == MotorState::STOP; + return joint->getMotor(degreeIndex)->getMotorState() == MotorState::STOP; } /** @@ -74,64 +98,110 @@ bool configMotorTarget(std::vector* names, std::vector* pos * @param as The Action Server this is occuring on */ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { - // For each point in the trajectory execution sequence... - for(int i = 0; i < goal->trajectory.points.size(); i++){ - // Capture the current goal for easy reference - trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; - - // Track whether or not the current position is done - bool hasPositionFinished = false; - // Capture the names of the motors - std::vector names; - // Capture the positions of the motors - std::vector positions; - // Keep max loop rate at 50 Hz - ros::Rate loop(200); - - // While the current position is not complete yet... - while(!hasPositionFinished){ - // Assume the current action is done until proven otherwise - hasPositionFinished = true; - // Create the Joint State message for the current update cycle - sensor_msgs::JointState js_msg; - - // Clear the list of motor names and position data - names.clear(); - positions.clear(); - - double velMax = 0; - // For each motor specified in the currTargetPosition... - for(int j = 0; j < currTargetPosition.positions.size()-2; j++){ - hasPositionFinished &= configMotorTarget(&names, &positions, currTargetPosition.positions[j], 0, motors[j]); + if (!IKEnabled) { + as->setAborted(); + ROS_WARN("IK is disabled"); + return; + } + + std::cout << "start exec: " << goal->trajectory.points.size() << std::endl; + // For each point in the trajectory execution sequence... + for(int i = 0; i < goal->trajectory.points.size(); i++){ + std::cout << "PID to trajectory point " << i << "/" << goal->trajectory.points.size() << std::endl; + // Capture the current goal for easy reference + trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; + + // Track whether or not the current position is done + bool hasPositionFinished = false; + // Keep max loop rate at 50 Hz + ros::Rate loop(200); + + // While the current position is not complete yet... + while(!hasPositionFinished){ + // Assume the current action is done until proven otherwise + hasPositionFinished = true; + // Create the Joint State message for the current update cycle + double velMax = abs(*std::max_element( + currTargetPosition.velocities.begin(), + currTargetPosition.velocities.end(), + [](double a, double b) { return abs(a)getDegreesOfFreedom(); k++){ + // Each motor should run to its respective target position at a fixed speed + // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters + double targetPos = currTargetPosition.positions[motorIndex]; + float currPower = 0.1 * currTargetPosition.velocities[j]/velMax; + + bool hasMotorFinished = configJointSetpoint(joint, motorIndex-jointIndex, targetPos, currPower); + hasPositionFinished &= hasMotorFinished; + motorIndex++; // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power - std::cout<getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()<getMotor(k)->getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<getPower()<exectute()) { + IKEnabled = false; + std_srvs::Trigger srv; + if (enableServiceClient.call(srv)) { + ROS_WARN((std::string{"PLACEHOLDER_NAME: "} + srv.response.message).data()); + } else { + ROS_WARN("Error: failed to call service PLACEHOLDER_NAME"); + } + return; + // TODO: hand control back to driver + // standard service empty stdsrvs empty + // go into spin loop } - - // config differential joint motors - std::vector motorTargets = convertJointSpacetoEncoderSpace(currTargetPosition.positions[5], currTargetPosition.positions[6]); - hasPositionFinished &= configMotorTarget(&names, &positions, motorTargets[0], 0, motors[5]); - hasPositionFinished &= configMotorTarget(&names, &positions, motorTargets[1], 0, motors[6]); - - // DEBUGGING OUTPUT: Print a divider line for cleanliness - std::cout<setSucceeded(); + for(int i = 0; i < numMotors; i++){ + motors[i]->setPower(0.f); + } + + as->setSucceeded(); +} + +/** + * @brief publishes the arm's position + */ +void publish(const ros::TimerEvent &event){ + std::vector names; + std::vector positions; + sensor_msgs::JointState js_msg; + + for (int i = 0; i < numMotors; i++){ + names.push_back(motors[i]->getMotorName()); + positions.push_back(motors[i]->getRads()); + } + + positions[4] = positions[5] + positions[4] / 2; + + js_msg.name = names; + js_msg.position = positions; + // Publish the Joint State message + jointStatePublisher.publish(js_msg); } /** @@ -143,20 +213,37 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server */ int main(int argc, char** argv) { + std::cout << "start main" << std::endl; // Initialize the current node as ArmControlSystem ros::init(argc, argv, "ArmControlSystem"); // Create the NodeHandle to the current ROS node ros::NodeHandle n; + ros::NodeHandle pn{"~"}; + + XmlRpcValue encParams; + pn.getParam("encoder_parameters", encParams); // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node - motors[0] = new ArmMotor("link1_joint", 0, 0, &n); - motors[1] = new ArmMotor("link2_joint", 0, 1, &n); - motors[2] = new ArmMotor("link3_joint", 1, 0, &n); - motors[3] = new ArmMotor("link4_joint", 1, 1, &n); - motors[4] = new ArmMotor("link5_joint", 2, 0, &n); - motors[5] = new ArmMotor("link6_joint", 2, 1, &n); - motors[6] = new ArmMotor("link7_joint", 3, 0, &n); + motors[0] = new ArmMotor("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); + motors[1] = new ArmMotor("forearm_roll", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); + motors[2] = new ArmMotor("shoulder", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); + motors[3] = new ArmMotor("turntable", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n); + motors[4] = new ArmMotor("wrist_pitch", 2, 0, static_cast(encParams[4]["counts_per_rotation"]), static_cast(encParams[4]["offset"]), n); + motors[5] = new ArmMotor("wrist_roll", 2, 1, static_cast(encParams[5]["counts_per_rotation"]), static_cast(encParams[5]["offset"]), n); + std::cout << "init motors" << std::endl; + // Initialize all Joints + joints[0] = new SimpleJoint(motors[0], &n); + joints[1] = new SimpleJoint(motors[1], &n); + joints[2] = new SimpleJoint(motors[2], &n); + joints[3] = new SimpleJoint(motors[3], &n); + joints[4] = new SimpleJoint(motors[4], &n); + DifferentialJoint* temp = new DifferentialJoint(motors[4], motors[5], &n); + temp->configVelocityHandshake("/control/arm/5/roll", "/control/arm/5/pitch", "/control/arm/20/", "/control/arm/21/"); + + std::cout << "init joints" << std::endl; + joints[5] = temp; + // Initialize the Joint State Data Publisher jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); @@ -164,11 +251,25 @@ int main(int argc, char** argv) Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); // Start the Action Server server.start(); + std::cout << "server started" << std::endl; + + ros::Timer timer = n.createTimer(ros::Duration(1.0 / 50.0), publish); - // signal(SIGINT, [](int signal)->void{ros::shutdown(); exit(1);}); + enableServiceServer = n.advertiseService("start_IK", + static_cast>( + [](std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)->bool{ + IKEnabled = true; + res.message = "Arm IK Enabled"; + res.success = true; + return true; + } + )); + enableServiceClient = n.serviceClient("PLACEHOLDER_NAME"); + + std::cout << "entering ROS spin..." << std::endl; // ROS spin for communication with other nodes ros::spin(); // Return 0 on exit (successful exit) return 0; -} \ No newline at end of file +} diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 6462f634..e9fc2267 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -7,34 +7,30 @@ #include "ArmMotor.hpp" /// Allow for referencing the UInt32 message type easier -#define Std_UInt32 std_msgs::UInt32::ConstPtr& -#define Std_Float64 std_msgs::Float64::ConstPtr& +typedef std_msgs::UInt32::ConstPtr Std_UInt32; +typedef std_msgs::Float64::ConstPtr Std_Float64; +#define Std_Bool std_msgs::Bool::ConstPtr& -/// The current COUNTS_PER_ROTATION is UINT32_MAX -uint32_t const ArmMotor::COUNTS_PER_ROTATION = UINT32_MAX; -/// The current encoders are absolute, and so can only perform one rotation -uint32_t const ArmMotor::ENCODER_BOUNDS[2] = {0, ArmMotor::COUNTS_PER_ROTATION}; - -template T ArmMotor::corrMod(T i, T j){ +double ArmMotor::corrMod(double i, double j){ // Stem i%j by j, which in modular arithmetic is the same as adding 0. - return fmod(fmod(i,j)+j,j); + return std::fmod(std::fmod(std::abs(j)*i/j,std::abs(j))+j,std::abs(j)); } /// Currently consistent with the rad->enc equation as specified here. -uint32_t ArmMotor::radToEnc(double rads){ - return ArmMotor::COUNTS_PER_ROTATION*ArmMotor::corrMod(rads,2 * M_PI)/(2 * M_PI); +uint32_t ArmMotor::radToEnc(double rads) const { + return this->COUNTS_PER_ROTATION * ArmMotor::corrMod(rads, 2 * M_PI)/(2 * M_PI) + this->ENCODER_OFFSET; } -double ArmMotor::encToRad(uint32_t enc){ - return ArmMotor::corrMod(enc / ((float)ArmMotor::COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; +double ArmMotor::encToRad(uint32_t enc) const { + return ArmMotor::corrMod(static_cast(enc - this->ENCODER_OFFSET) / static_cast(this->COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; } /// Currently consistent with the enc->rad equation as specified here. -float ArmMotor::getRads(){ +double ArmMotor::getRads() const{ return ArmMotor::encToRad(this->getEncoderCounts()); } -void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ +void ArmMotor::storeEncoderVals(const Std_UInt32 &msg){ // Store the message value in this ArmMotor's encoderVal variable this->encoderVal = msg->data; // Send feedback @@ -43,63 +39,76 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 msg){ this->feedbackPub.publish(feedbackMsg); } -void ArmMotor::redirectPowerOutput(const Std_Float64 msg){ +void ArmMotor::redirectPowerOutput(const Std_Float64 &msg){ // Set the speed to be the contained data this->setPower(msg->data); } +void ArmMotor::storeStallStatus(const Std_Bool msg) { + this->isStall = msg->data; +} + /// controllerID is constrained between [0,3] /// motorID is constrained between [0,1] -ArmMotor::ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n){ +ArmMotor::ArmMotor( + const std::string &motorName, + unsigned int controllerID, + unsigned int motorID, + int64_t countsPerRotation, + int64_t offset, + ros::NodeHandle &n +) : COUNTS_PER_ROTATION{countsPerRotation}, + ENCODER_BOUNDS{0, std::abs(countsPerRotation)}, + ENCODER_OFFSET{offset}, + motorName{motorName}, + controllerID{controllerID}, + motorID{motorID}, + currState{MotorState::STOP}, + encoderVal{static_cast(offset)} { + // Check validity of WRoboclaw and motor IDs if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; - - // Set the current name, controller, and motor ID - this->motorName = motorName; - this->controllerID = controllerID; - this->motorID = motorID; - // All motors start STOPped - this->currState = MotorState::STOP; // Create the topic string prefix for WRoboclaw controllers std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); std::string controlString = "/control/arm/" + std::to_string(controllerID) + std::to_string(motorID); // Create the appropriate encoder-reading and speed-publishing subscribers and advertisers, respectfully - this->encRead = n->subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), 1000, &ArmMotor::storeEncoderVals, this); - this->speedPub = n->advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), 1000); - this->targetPub = n->advertise(controlString + "/setpoint", 1000); - this->feedbackPub = n->advertise(controlString + "/feedback", 1000); - this->outputRead = n->subscribe(controlString + "/output", 1000, &ArmMotor::redirectPowerOutput, this); + this->encRead = n.subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::storeEncoderVals, this); + this->speedPub = n.advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE); + this->targetPub = n.advertise(controlString + "/setpoint", ArmMotor::MESSAGE_CACHE_SIZE); + this->feedbackPub = n.advertise(controlString + "/feedback", ArmMotor::MESSAGE_CACHE_SIZE); + this->outputRead = n.subscribe(controlString + "/output", ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::redirectPowerOutput, this); + this->stallRead = n.subscribe(tpString + "/curr/over_lim/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::storeStallStatus, this); } -uint32_t ArmMotor::getEncoderCounts(){ +uint32_t ArmMotor::getEncoderCounts() const{ return this->encoderVal; } void ArmMotor::runToTarget(uint32_t targetCounts, float power){ + std::cout << "run to enc: " << targetCounts << std::endl; this->runToTarget(targetCounts, power, false); } -bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance){ +bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const { // Compute the upper and lower bounds in the finite encoder space - uint32_t lBound = ArmMotor::corrMod(targetCounts - tolerance, ArmMotor::ENCODER_BOUNDS[1]); - uint32_t uBound = ArmMotor::corrMod(targetCounts + tolerance, ArmMotor::ENCODER_BOUNDS[1]); + uint32_t lBound = ArmMotor::corrMod(static_cast(targetCounts - tolerance), static_cast(ArmMotor::ENCODER_BOUNDS[1])); + uint32_t uBound = ArmMotor::corrMod(static_cast(targetCounts + tolerance), static_cast(ArmMotor::ENCODER_BOUNDS[1])); // If the computed lower bound is lower than the upper bound, perform the computation normally if(lBound < uBound) return this->getEncoderCounts() <= uBound && this->getEncoderCounts() >=lBound; // Otherwise, check if the value is outside either bound and negate the response - else - return this->getEncoderCounts() <= uBound || this->getEncoderCounts() >=lBound; + return this->getEncoderCounts() <= uBound || this->getEncoderCounts() >=lBound; } /// Current tolerance is ±0.1 degree w.r.t. the current number of counts per rotation -bool ArmMotor::hasReachedTarget(uint32_t targetCounts){ - return ArmMotor::hasReachedTarget(targetCounts, 1200000); +bool ArmMotor::hasReachedTarget(uint32_t targetCounts) const { + return ArmMotor::hasReachedTarget(targetCounts, ArmMotor::TOLERANCE_RATIO * static_cast(std::abs(this->COUNTS_PER_ROTATION))); } -MotorState ArmMotor::getMotorState(){ +MotorState ArmMotor::getMotorState() const { return this->currState; } @@ -109,43 +118,69 @@ void ArmMotor::setPower(float power){ if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; // Set up and send the speed message - this->powerMsg = new std_msgs::Int16(); - this->powerMsg->data = power * INT16_MAX; - this->speedPub.publish(*(this->powerMsg)); - // Update the current motor state based on the power command input - this->currState = power == 0.f ? MotorState::STOP : MotorState::MOVING; -} - -void ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ + this->powerMsg.data = power * INT16_MAX; + this->speedPub.publish(this->powerMsg); + // Update the cur.nt motor state based on the power command input + this->currState = power == 0.F ? MotorState::STOP : MotorState::MOVING; +} + +bool ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ + // Checks for stall + if (this->isStall) { + if ((ros::Time::now() - begin).toSec() >= 0.5) { + this->setPower(0.0f); + this->currState = MotorState::STOP; + return false; + } + } else { + begin = ros::Time::now(); + } + // If we are not at our target... if(!this->hasReachedTarget(targetCounts)){ // Set the power in the correct direction and continue running to the target std_msgs::Float64 setpointMsg; setpointMsg.data = ArmMotor::encToRad(targetCounts); this->targetPub.publish(setpointMsg); + + // long int direction = targetCounts - this->getEncoderCounts(); + // power = abs(power) * (corrMod(direction, ((long int)this->COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)this->COUNTS_PER_ROTATION)) ? 1 : -1); + // this->setPower(power); + this->currState = MotorState::RUN_TO_TARGET; // Otherwise, stop the motor - }else{ - this->setPower(0.f); + } else { + this->setPower(0.F); this->currState = MotorState::STOP; } // If this is a blocking call... if(block){ // Wait until the motor has reached the target, then stop while(!this->hasReachedTarget(targetCounts)); - this->setPower(0.f); + this->setPower(0.F); this->currState = MotorState::STOP; } + return true; } -void ArmMotor::runToTarget(double rads, float power){ - runToTarget(this->radToEnc(rads), power, false); +bool ArmMotor::runToTarget(double rads, float power){ + std::cout << "run to target: " << rads << ":" << this->radToEnc(rads) << std::endl; + + return runToTarget(this->radToEnc(rads), power, false); } -std::string ArmMotor::getMotorName(){ +std::string ArmMotor::getMotorName() const { return this->motorName; } -float ArmMotor::getPower(){ - return ((float)this->powerMsg->data)/INT16_MAX; +unsigned int ArmMotor::getMotorID() const { + return this->motorID; +} + +unsigned int ArmMotor::getControllerID() const { + return this->controllerID; +} + +float ArmMotor::getPower() const { + return ((float)this->powerMsg.data)/INT16_MAX; } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 1819e217..f61b7979 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -9,6 +9,7 @@ #include "std_msgs/UInt32.h" #include "std_msgs/Int16.h" #include "std_msgs/Float64.h" +#include "std_msgs/Bool.h" #include "math.h" #include @@ -28,10 +29,16 @@ enum class MotorState{ */ class ArmMotor{ private: + /// Tolerance ratio w.r.t the counts per rotation for encoder run-to-position motion + static constexpr double TOLERANCE_RATIO = 0.1/360; + /// Size of ROS Topic message caches + static constexpr uint32_t MESSAGE_CACHE_SIZE = 10; /// The number of encoder counts per rotation - static uint32_t const COUNTS_PER_ROTATION; + int64_t const COUNTS_PER_ROTATION; /// The upper and lower bounds of encoder rotation for absolute encoders (index 0 is lower, index 1 is upper) - static uint32_t const ENCODER_BOUNDS[2]; + std::array const ENCODER_BOUNDS; + /// zero position of motor + int64_t const ENCODER_OFFSET; /// The current state of the motor MotorState currState; /// The joint name of the current motor @@ -52,8 +59,14 @@ class ArmMotor{ ros::Subscriber outputRead; /// The ROS Publisher that publishes motor speed commands ros::Publisher speedPub; - /// A pointer to the most recent power message sent - std_msgs::Int16 *powerMsg; + /// The most recent power message sent + std_msgs::Int16 powerMsg; + /// If the motor is stalling or not + volatile bool isStall; + /// The ROS Subscriber that reads stall status data + ros::Subscriber stallRead; + /// The time when the motor began stalling + ros::Time begin; /** * @brief A static conversion from radians to encoder counts @@ -61,7 +74,7 @@ class ArmMotor{ * @param rad The input number of radians * @return uint32_t The corresponding encoder count bounded by ENCODER_BOUNDS */ - static uint32_t radToEnc(double rad); + uint32_t radToEnc(double rad) const; /** * @brief A static conversion from encoder counts to radians @@ -69,7 +82,7 @@ class ArmMotor{ * @param enc The input number of encoder counts * @return double The corresponding radian measure */ - static double encToRad(uint32_t enc); + double encToRad(uint32_t enc) const; /** * @brief Subscriber callback for encRead, captures the encoder value of the current motor @@ -85,15 +98,21 @@ class ArmMotor{ */ void redirectPowerOutput(const std_msgs::Float64::ConstPtr& msg); + /** + * @brief Subscriber callback for stallRead, captures the stall status of the current motor + * + * @param msg The stall status of the current motor + */ + void storeStallStatus(const std_msgs::Bool::ConstPtr& msg); + /** * @brief Performs Euclidean correct modulus between two inputs of the same type * - * @tparam T Some type on which addition and comparisons occur * @param i The dividend of the modulus * @param j The divisor of the modulus - * @return T The Euclidean-correct remainder bounded on [0, j) + * @return double The Euclidean-correct remainder bounded on [0, j) */ - template static T corrMod(T i, T j); + static double corrMod(double i, double j); public: /** @@ -104,14 +123,14 @@ class ArmMotor{ * @param motorID The motor ID within its WRoboclaw controller * @param n A NodeHandle reference to the constructing Node */ - ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, ros::NodeHandle* n); + ArmMotor(const std::string &motorName, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, int64_t offset, ros::NodeHandle &n); /** * @brief Gets the encoder value of the motor * * @return uint32_t The current encoder value of the motor */ - uint32_t getEncoderCounts(); + uint32_t getEncoderCounts() const; /** * @brief Sends the motor to run to a target encoder value at a given power without blocking @@ -127,23 +146,25 @@ class ArmMotor{ * @param targetCounts The target encoder value for the motor * @param power The power to move the motor at (Bounded between [-1, 1]) * @param block Specifies whether or not this action should block until it is complete + * @return True if the motor had stalled, and false otherwise */ - void runToTarget(uint32_t targetCounts, float power, bool block); + bool runToTarget(uint32_t targetCounts, float power, bool block); /** * @brief Sends the motor to run to a specified position at a given power * * @param rads The position to send the motor to (specified in radians) * @param power The power to move the motor at (Bounded between [-1, 1]) + * @return True if the motor had stalled, and false otherwise */ - void runToTarget(double rads, float power); + bool runToTarget(double rads, float power); /** * @brief Get the current state of the ArmMotor * * @return MotorState The current state of the ArmMotor */ - MotorState getMotorState(); + MotorState getMotorState() const; /** * @brief Set the motor power @@ -155,16 +176,31 @@ class ArmMotor{ /** * @brief Get the radian measure of the current motor * - * @return float The radian measure of the current motor's position + * @return double The radian measure of the current motor's position */ - float getRads(); + double getRads() const; /** * @brief Get the name of the ArmMotor * * @return std::string The name of the ArmMotor */ - std::string getMotorName(); + std::string getMotorName() const; + + /** + * @brief Get the name of the ArmMotor + * + * @return std::string The controller ID of the ArmMotor + */ + unsigned int getControllerID() const; + + /** + * @brief Get the name of the ArmMotor + * + * @return std::string The motor ID of the ArmMotor + */ + unsigned int getMotorID() const; + /** * @brief Checks if the motor is currently within a pre-specified tolerance of a target @@ -173,7 +209,7 @@ class ArmMotor{ * @return true The motor was within the target tolerance * @return false The motor was outside of the target tolerance */ - bool hasReachedTarget(uint32_t targetCounts); + bool hasReachedTarget(uint32_t targetCounts) const; /** * @brief Checks if the motor is currently within a given tolerance of a target @@ -183,6 +219,12 @@ class ArmMotor{ * @return true The motor was within the target tolerance * @return false The motor was outside the target tolerance */ - bool hasReachedTarget(uint32_t targetCounts, uint32_t tolerance); - float getPower(); + bool hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const; + + /** + * @brief Get the most recently set motor power + * + * @return float Last motor power setting + */ + float getPower() const; }; diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp new file mode 100644 index 00000000..ef600304 --- /dev/null +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -0,0 +1,122 @@ +/** + * @file AbstractJoint.hpp + * @author Nichols Underwood + * @brief ablskjlfkejfs + * @date 2021-10-25 + */ + +#include "DifferentialJoint.hpp" +#include +using std::vector; + +DifferentialJoint::DifferentialJoint(ArmMotor* leftMotor, ArmMotor* rightMotor, ros::NodeHandle* n) : AbstractJoint(n, 2) { + this->motors.push_back(leftMotor); + this->motors.push_back(rightMotor); +} + +void DifferentialJoint::getJointPositions(vector &motorPositions, vector &target){ + // vector positions; + // target->reserve(2); + + double pitch = motorPositions[0] * this->motorToJointMatrix[0][0] + motorPositions[1]*this->motorToJointMatrix[0][1]; + double roll = motorPositions[1] * this->motorToJointMatrix[1][0] + motorPositions[1]*this->motorToJointMatrix[1][1]; + + target.push_back(pitch); + target.push_back(roll); + + // return positions; +} + +void DifferentialJoint::getMotorPositions(vector &jointPositions, vector &target){ + + // std::unique_ptr> positions = std::make_unique>(2); + // target->reserve(2); + + double left = jointPositions[0] * this->jointToMotorMatrix[0][0] + jointPositions[1]*this->jointToMotorMatrix[0][1]; + double right = jointPositions[1] * this->jointToMotorMatrix[1][0] + jointPositions[1]*this->jointToMotorMatrix[1][1]; + + target.push_back(left); + target.push_back(right); + + // return std::move(positions); +} + +void DifferentialJoint::getMotorVelocities(vector &jointPositions, vector &target){ + return getMotorPositions(jointPositions, target); //deritivate of linear transformation is itself +} + +void DifferentialJoint::configVelocityHandshake(std::string pitchTopicName, std::string rollTopicName, std::string leftTopicName, std::string rightTopicName){ + this->pitchOutputSubscriber = this->n->subscribe(pitchTopicName + "/output", 1000, &DifferentialJoint::handoffPitchOutput, this); + this->rollOutputSubscriber = this->n->subscribe(rollTopicName + "/output", 1000, &DifferentialJoint::handoffRollOutput, this); + this->leftOutputPublisher = this->n->advertise(leftTopicName + "/output", 1000); + this->rightOutputPublisher = this->n->advertise(rightTopicName + "/output", 1000); + this->leftFeedbackSubscriber = this->n->subscribe(leftTopicName + "/feeback", 1000, &DifferentialJoint::handoffLeftFeedback, this); + this->rightFeedbackSubscriber = this->n->subscribe(rightTopicName + "/feeback", 1000, &DifferentialJoint::handoffRightFeedback, this); + this->pitchFeedbackPublisher = this->n->advertise(pitchTopicName + "/feedback", 1000); + this->rollFeedbackPublisher = this->n->advertise(rollTopicName + "/feedback", 1000); +} + +void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64 msg){ + this->cachedPitchOutput = msg.data; + this->hasNewPitchOutput = true; + handOffAllOutput(); +} +void DifferentialJoint::handoffRollOutput(const std_msgs::Float64 msg){ + this->cachedRollOutput = msg.data; + this->hasNewRollOutput = true; + handOffAllOutput(); +} + +void DifferentialJoint::handOffAllOutput(){ + if(!this->hasNewPitchOutput || !this->hasNewRollOutput){ return; } + + vector outputs; + outputs[0] = this->cachedPitchOutput; + outputs[1] = this->cachedRollOutput; + getMotorVelocities(outputs, outputs); + + std_msgs::Float64 msg1 = std_msgs::Float64(); + std_msgs::Float64 msg2 = std_msgs::Float64(); + msg1.data = outputs[0]; + msg2.data = outputs[1]; + pitchFeedbackPublisher.publish(msg1); + rollFeedbackPublisher.publish(msg2); + + leftOutputPublisher.publish(msg1); + rightOutputPublisher.publish(msg2); + + this->hasNewPitchOutput = false; + this->hasNewRollOutput = false; + +} + +void DifferentialJoint::handoffRightFeedback(const std_msgs::Float64 msg){ + this->cachedRightFeedback = msg.data; + this->hasNewRightFeedback = true; + handOffAllFeedback(); +} +void DifferentialJoint::handoffLeftFeedback(const std_msgs::Float64 msg){ + this->cachedLeftFeedback = msg.data; + this->hasNewLeftFeedback = true; + handOffAllFeedback(); +} + +void DifferentialJoint::handOffAllFeedback(){ + if(!this->hasNewLeftFeedback || !this->hasNewRightFeedback){ return; } + + vector outputs; + outputs[0] = this->cachedLeftFeedback; + outputs[1] = this->cachedRightFeedback; + getMotorVelocities(outputs, outputs); + + std_msgs::Float64 msg1 = std_msgs::Float64(); + std_msgs::Float64 msg2 = std_msgs::Float64(); + msg1.data = outputs[0]; + msg2.data = outputs[1]; + pitchFeedbackPublisher.publish(msg1); + rollFeedbackPublisher.publish(msg2); + + this->hasNewLeftFeedback = false; + this->hasNewRightFeedback = false; + +} diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp new file mode 100644 index 00000000..07606c2a --- /dev/null +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -0,0 +1,48 @@ +#include "AbstractJoint.hpp" + +class DifferentialJoint : public AbstractJoint { + public: + ~DifferentialJoint(); + DifferentialJoint(ArmMotor *leftMotor, ArmMotor *rightMotor, ros::NodeHandle *n); + + void getMotorPositions(vector &jointPositions, vector &target); + void getMotorVelocities(vector &jointVelocities, vector &target); + void getJointPositions(vector &motorPositions, vector &target); + + void configVelocityHandshake(std::string pitchTopicName, std::string rollTopicName, std::string leftTopicName, std::string rightTopicName); + + private: + // linear transformations works for position and velocity + // [0.5 0.5] [left motor ] [pitch] + // [ -1 1 ] * [right motor] = [roll ] + // double motorToJointMatrix[2][2] = {{0.5, 0.5}, {-1, 1}}; + double motorToJointMatrix[2][2] = {{1, 0}, {0, 1}}; + // [1 -0.5] [pitch] [left motor ] + // [1 0.5] * [roll ] = [right motor] + // double jointToMotorMatrix[2][2] = {{1, 0.5}, {1, -0.5}}; + double jointToMotorMatrix[2][2] = {{1, 0}, {0, 1.0}}; + + void handoffPitchOutput(std_msgs::Float64); + void handoffRollOutput(std_msgs::Float64); + void handOffAllOutput(); + ros::Subscriber pitchOutputSubscriber; + ros::Subscriber rollOutputSubscriber; + ros::Publisher leftOutputPublisher; + ros::Publisher rightOutputPublisher; + float cachedPitchOutput = 0.0; + float cachedRollOutput = 0.0; + bool hasNewPitchOutput = false; + bool hasNewRollOutput = false; + + void handoffLeftFeedback(std_msgs::Float64); + void handoffRightFeedback(std_msgs::Float64); + void handOffAllFeedback(); + ros::Subscriber leftFeedbackSubscriber; + ros::Subscriber rightFeedbackSubscriber; + ros::Publisher pitchFeedbackPublisher; + ros::Publisher rollFeedbackPublisher; + float cachedLeftFeedback = 0.0; + float cachedRightFeedback = 0.0; + bool hasNewLeftFeedback = false; + bool hasNewRightFeedback = false; +}; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleACS.cpp b/src/wr_control_drive_arm/src/SimpleACS.cpp deleted file mode 100644 index 8417a880..00000000 --- a/src/wr_control_drive_arm/src/SimpleACS.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "ros/ros.h" -#include -#include -#include -#include -#include "ArmMotor.hpp" - -ArmMotor* motors[7]; -ros::Publisher jointStatePublisher; -typedef actionlib::SimpleActionServer Server; - -void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { - // for(int i = 0; i < goal->trajectory.points.size(); i++){ - // trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; - - - // } - motors[0]->setPower(0.01); - int i = 0; - ros::Rate sleeper(50); - while(i++ < 100){ - // std::cout<getEncoderCounts()<getPower()<setSucceeded(); -} - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "ArmControlSystem"); - ros::NodeHandle n; - - motors[0] = new ArmMotor("link1_joint", 0, 0, &n); - motors[1] = new ArmMotor("link2_joint", 0, 1, &n); - motors[2] = new ArmMotor("link3_joint", 1, 0, &n); - motors[3] = new ArmMotor("link4_joint", 1, 1, &n); - motors[4] = new ArmMotor("link5_joint", 2, 0, &n); - motors[5] = new ArmMotor("link6_joint", 2, 1, &n); - motors[6] = new ArmMotor("link7_joint", 3, 0, &n); - - jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); - Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); - - server.start(); - ros::spin(); - return 0; -} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.cpp b/src/wr_control_drive_arm/src/SimpleJoint.cpp new file mode 100644 index 00000000..b846f219 --- /dev/null +++ b/src/wr_control_drive_arm/src/SimpleJoint.cpp @@ -0,0 +1,48 @@ +#ifndef SIMPLE_JOINT_GUARD +#define SIMPLE_JOINT_GUARD +/** + * @file AbstractJoint.cpp + * @author Nichols Underwood + * @brief ablskjlfkejfs + * @date 2021-10-25 + */ + +#include "SimpleJoint.hpp" + +SimpleJoint::SimpleJoint(ArmMotor* motor, ros::NodeHandle *n) : AbstractJoint(n, 1) { + this->motors.push_back(motor); +} + +void SimpleJoint::getJointPositions(vector &motorPositions, vector &target){ + // vector positions = *(new vector()); // makes the red lines go away + + // target->reserve(1); + target.push_back(motorPositions[0]); + + // return positions; +} + +void SimpleJoint::getMotorPositions(vector &jointPositions, vector &target){ + + target.reserve(1); + double position = jointPositions[0]; + target.push_back(position); + // return positions; +} + +void SimpleJoint::getMotorVelocities(vector &jointPositions, vector &target){ + + // target->reserve(1); + target.push_back(jointPositions[0]); + + // return setpoints; +} + +void SimpleJoint::handoffOutput(const std_msgs::Float64 msg){ + this->motorOutputPublisher.publish(msg); +} + +void SimpleJoint::handoffFeedback(const std_msgs::Float64 msg){ + this->jointFeedbackPublisher.publish(msg); +} +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.hpp b/src/wr_control_drive_arm/src/SimpleJoint.hpp new file mode 100644 index 00000000..9be7198d --- /dev/null +++ b/src/wr_control_drive_arm/src/SimpleJoint.hpp @@ -0,0 +1,28 @@ +#include "AbstractJoint.hpp" +using std::vector; + +class SimpleJoint : public AbstractJoint { + public: + ~SimpleJoint(); + SimpleJoint(ArmMotor* motor, ros::NodeHandle* n); + + int a(){return 2;} + int b(){return 3;} + + void getMotorPositions(vector &jointPositions, vector &target); + void getMotorVelocities(vector &ointVelocities, vector &target); + void getJointPositions(vector &motorPositions, vector &target); + + void configMotor(ArmMotor* motor); + void configVelocityHandshake(std::string jointTopicName, std::string motorTopicName); + void handoffOutput(std_msgs::Float64); + void handoffFeedback(std_msgs::Float64); + + private: + ros::Subscriber jointOutputSubscriber; + ros::Publisher motorOutputPublisher; + + ros::Subscriber motorFeedbackSubscriber; + ros::Publisher jointFeedbackPublisher; + +}; diff --git a/src/wr_entry_point/launch/comp/comp_arm.launch b/src/wr_entry_point/launch/comp/comp_arm.launch index 8f356b47..1d537d63 100644 --- a/src/wr_entry_point/launch/comp/comp_arm.launch +++ b/src/wr_entry_point/launch/comp/comp_arm.launch @@ -3,7 +3,7 @@ - + diff --git a/src/wr_entry_point/launch/mock_arm.launch b/src/wr_entry_point/launch/mock_arm.launch new file mode 100644 index 00000000..69547c0a --- /dev/null +++ b/src/wr_entry_point/launch/mock_arm.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/src/wr_entry_point/launch/test/test_arm.launch b/src/wr_entry_point/launch/test/test_arm.launch index 16dd0cf9..aeac344c 100644 --- a/src/wr_entry_point/launch/test/test_arm.launch +++ b/src/wr_entry_point/launch/test/test_arm.launch @@ -1,6 +1,8 @@ + + @@ -8,7 +10,11 @@ - + - + + + + + diff --git a/src/wr_entry_point/launch/test/vis_arm.launch b/src/wr_entry_point/launch/test/vis_arm.launch new file mode 100644 index 00000000..abc1bf63 --- /dev/null +++ b/src/wr_entry_point/launch/test/vis_arm.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + diff --git a/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml b/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml index 11268a6f..c263a2c6 100644 --- a/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml +++ b/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml @@ -6,37 +6,37 @@ claws: address: 0x80 enc_left: enabled: true - max_speed: 300000000 # TODO probably don't actually need all these encoders + max_speed: 100 # TODO probably don't actually need all these encoders enc_right: enabled: true - max_speed: 300000000 + max_speed: 100 aux1: # arm elbow/forearm, science ? topic: '/hsi/roboclaw/aux1' address: 0x81 enc_left: enabled: true - max_speed: 300000000 + max_speed: 100 enc_right: enabled: true - max_speed: 300000000 + max_speed: 100 aux2: # arm wrist, science ? topic: '/hsi/roboclaw/aux2' address: 0x82 enc_left: enabled: true - max_speed: 300000000 + max_speed: 100 enc_right: enabled: true - max_speed: 300000000 + max_speed: 100 aux3: # arm manip, science ? topic: '/hsi/roboclaw/aux3' address: 0x83 enc_left: enabled: true - max_speed: 300000000 + max_speed: 100 enc_right: enabled: true - max_speed: 300000000 + max_speed: 100 drive: # drive train topic: '/hsi/roboclaw/drive' address: 0x84 diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index 1f29acd5..52bfbb9e 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -29,9 +29,8 @@ auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orienta p.pose.position.z = z_pos; auto outOrientation = /* tf2::Quaternion(0,sin(M_PI/4), 0, cos(M_PI/4)) * */ orientation; p.pose.orientation = tf2::toMsg(outOrientation); - p.header.frame_id = "turntable"; + p.header.frame_id = "base_link"; pub.publish(p); - // isNewPath.store(true); } auto main(int argc, char** argv) -> int{ @@ -44,16 +43,21 @@ auto main(int argc, char** argv) -> int{ constexpr float CLOCK_RATE = 2; constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000; constexpr float TRIGGER_PRESSED = 0.5; + constexpr float JOYSTICK_DEADBAND = 0.1; constexpr float MINIMUM_PATH_ACCURACY = 0.0; - constexpr float STEP_X = 0.001; - constexpr float STEP_Y = 0.001; - constexpr float STEP_Z = 0.001; + constexpr float STEP_X = 0.003; + constexpr float STEP_Y = 0.003; + constexpr float STEP_Z = 0.003; - constexpr float HOME_X = 0.25; - constexpr float HOME_Y = 0; - constexpr float HOME_Z = 0.25; + constexpr float HOME_X = -0.7; + constexpr float HOME_Y = 0.0; + constexpr float HOME_Z = 0.2; + + float accel = 1.0; + float deaccel = 1.0; + float step_mult = 1.0; float x_pos = HOME_X; float y_pos = HOME_Y; float z_pos = HOME_Z; @@ -70,88 +74,158 @@ auto main(int argc, char** argv) -> int{ move.setPlanningTime(PLANNING_TIME); const moveit::core::JointModelGroup* joint_model_group = move.getCurrentState()->getJointModelGroup("arm"); robot_state::RobotState start_state(*move.getCurrentState()); + moveit_visual_tools::MoveItVisualTools visual_tools("arm"); + ros::Rate loop {CLOCK_RATE}; ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", MESSAGE_QUEUE_LENGTH); - - ros::Subscriber yAxis = np.subscribe("/xbox_test/axis/pov_y", + + ros::Publisher trajectoryPub = np.advertise("/logic/arm_teleop/trajectory", + MESSAGE_QUEUE_LENGTH); + + + // y axis + ros::Subscriber yAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_y", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { - if(msg->data != 0){ - y_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_Y : -STEP_Y : 0; + if(abs(msg->data) >= JOYSTICK_DEADBAND){ + y_pos += msg->data * STEP_Y; updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber xAxis = np.subscribe("/xbox_test/axis/pov_x", + // x axis + ros::Subscriber xAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_x", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { - if(msg->data != 0){ - x_pos += static_cast(msg->data) ? msg->data > 0 ? STEP_X : -STEP_X : 0; + if(abs(msg->data) >= JOYSTICK_DEADBAND){ + x_pos += msg->data * STEP_X; updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber zUp = np.subscribe("/xbox_test/button/shoulder_l", + // y up + ros::Subscriber zUp = np.subscribe("/hci/arm/gamepad/button/shoulder_l", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Bool msg) -> void { if(msg->data){ - z_pos += msg->data ? STEP_Z : 0; + z_pos += STEP_Y * step_mult; updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber zDown = np.subscribe("/xbox_test/axis/trigger_left", + // y down + ros::Subscriber zDown = np.subscribe("/hci/arm/gamepad/button/shoulder_r", MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - if(msg->data >= TRIGGER_PRESSED){ - z_pos += msg->data >= TRIGGER_PRESSED ? -STEP_Z : 0; + static_cast>([&](Std_Bool msg) -> void { + if(msg->data){ + z_pos -= STEP_Y * step_mult; updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber roll = np.subscribe("/xbox_test/axis/stick_left_x", + //roll counter clockwise + ros::Subscriber rollUp = np.subscribe("/hci/arm/gamepad/button/y", MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - if(abs(msg->data) >= 0.5){ - orientation *= (msg->data > 0 ? SPIN_Z : SPIN_Z.inverse()); + static_cast>([&](Std_Bool msg) -> void { + if(msg->data){ + orientation *= SPIN_Z * step_mult; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + + } + })); + + // roll clockwise + ros::Subscriber rollDown = np.subscribe("/hci/arm/gamepad/button/a", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if(msg->data){ + orientation *= SPIN_Z.inverse(); updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber pitch = np.subscribe("/xbox_test/axis/stick_left_y", + // pitch + ros::Subscriber pitch = np.subscribe("/hci/arm/gamepad/axis/stick_right_y", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { if(abs(msg->data) >= 0.5){ orientation *= (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse()); updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber yaw = np.subscribe("/xbox_test/axis/stick_right_x", + // yaw + ros::Subscriber yaw = np.subscribe("/hci/arm/gamepad/axis/stick_right_x", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Float32 msg) -> void { if(abs(msg->data) >= 0.5){ orientation *= (msg->data > 0 ? SPIN_X : SPIN_X.inverse()); updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } })); - ros::Subscriber execPath = np.subscribe("/xbox_test/button/a", + ros::Subscriber speedUp = np.subscribe("/hci/arm/gamepad/axis/trigger_right", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + accel = msg->data; + step_mult = std::pow(10,accel-deaccel); + })); + + ros::Subscriber speedDown = np.subscribe("/hci/arm/gamepad/axis/trigger_right", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + deaccel = msg->data; + step_mult = std::pow(10,accel-deaccel); + })); + + // override path execution + ros::Subscriber execPath = np.subscribe("/hci/arm/gamepad/button/x", MESSAGE_QUEUE_LENGTH, static_cast>([&](Std_Bool msg) -> void { if(msg->data){ isNewPath.store(true); } })); - - // transform = move.getCurrentState()->getFrameTransform("odom_combined"); - // updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + + // reset target/cancel path + ros::Subscriber resetPose = np.subscribe("/hci/arm/gamepad/button/start", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if(msg->data){ + + geometry_msgs::Pose pose = move.getCurrentPose().pose; + x_pos = pose.position.x; + y_pos = pose.position.y; + z_pos = pose.position.z; + tf2::convert(pose.orientation, orientation); + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + + isNewPath.store(true); //override path + } + })); + while(ros::ok()){ + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); if(!isNewPath.load()){ loop.sleep(); continue; @@ -168,16 +242,15 @@ auto main(int argc, char** argv) -> int{ p.pose.position.z = z_pos; p.pose.orientation = tf2::toMsg(orientation); p.header.frame_id = "odom_combined"; + move.setPoseTarget(p); - std::vector waypoints {p.pose}; - moveit_msgs::RobotTrajectory traj; + move.setStartStateToCurrentState(); //plan and execute path - move.setStartStateToCurrentState(); move.asyncMove(); while(ros::ok){ - // std::cout << "[INFO] [" << ros::Time::now() << "]: executing path " << std::endl; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); if(move.getMoveGroupClient().getState().isDone()){ std::cout << "[INFO] [" << ros::Time::now() << "]: path finished: " << move.getMoveGroupClient().getState().getText() << std::endl; diff --git a/src/wroboarm_22/.setup_assistant b/src/wroboarm_22/.setup_assistant new file mode 100644 index 00000000..85e8ffb2 --- /dev/null +++ b/src/wroboarm_22/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: wroboarm_22 + relative_path: urdf/wroboarm_22.urdf + xacro_args: "" + SRDF: + relative_path: config/wroboarm_22.srdf + CONFIG: + author_name: Nicholas Underwood + author_email: ndunderwood@wisc.edu + generated_timestamp: 1643850775 \ No newline at end of file diff --git a/src/wroboarm_22/CMakeLists.txt b/src/wroboarm_22/CMakeLists.txt new file mode 100644 index 00000000..0d3118b1 --- /dev/null +++ b/src/wroboarm_22/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(wroboarm_22) + +find_package(catkin REQUIRED) + +catkin_package() + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/src/wroboarm_22/config/arm_assembly.srdf b/src/wroboarm_22/config/arm_assembly.srdf new file mode 100644 index 00000000..f9dfd6b0 --- /dev/null +++ b/src/wroboarm_22/config/arm_assembly.srdf @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/config/cartesian_limits.yaml b/src/wroboarm_22/config/cartesian_limits.yaml new file mode 100644 index 00000000..7df72f69 --- /dev/null +++ b/src/wroboarm_22/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/src/wroboarm_22/config/chomp_planning.yaml b/src/wroboarm_22/config/chomp_planning.yaml new file mode 100644 index 00000000..75258e53 --- /dev/null +++ b/src/wroboarm_22/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/src/wroboarm_22/config/fake_controllers.yaml b/src/wroboarm_22/config/fake_controllers.yaml new file mode 100644 index 00000000..0ceba3e3 --- /dev/null +++ b/src/wroboarm_22/config/fake_controllers.yaml @@ -0,0 +1,15 @@ +controller_list: + - name: fake_arm_controller + type: $(arg execution_type) + joints: + - turntable + - shoulder + - elbow + - forearm_roll + - wrist_pitch + - wrist_roll +initial: # Define initial robot poses. +# - group: arm +# pose: home + + [] \ No newline at end of file diff --git a/src/wroboarm_22/config/gazebo_controllers.yaml b/src/wroboarm_22/config/gazebo_controllers.yaml new file mode 100644 index 00000000..e4d2eb00 --- /dev/null +++ b/src/wroboarm_22/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/src/wroboarm_22/config/joint_limits.yaml b/src/wroboarm_22/config/joint_limits.yaml new file mode 100644 index 00000000..b9def49a --- /dev/null +++ b/src/wroboarm_22/config/joint_limits.yaml @@ -0,0 +1,52 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 1.0 +default_acceleration_scaling_factor: 1.0 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + elbow: + max_position: 0.436 + min_position: -2.5307 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + forearm_roll: + max_position: 1.57 + min_position: -1.57 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + shoulder: + max_position: 2.2689 + min_position: -0.87266 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + turntable: + max_position: 3.1 + min_position: -3.1 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + wrist_pitch: + max_position: 1.57 + min_position: -1.57 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + wrist_roll: + max_position: 1.57 + min_position: -1.57 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/src/wroboarm_22/config/joint_names_wroboarm_22.yaml b/src/wroboarm_22/config/joint_names_wroboarm_22.yaml new file mode 100644 index 00000000..159fc6fe --- /dev/null +++ b/src/wroboarm_22/config/joint_names_wroboarm_22.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'turntable', 'shoulder', 'elbow', 'forearm_roll', 'wrist_pitch', 'wrist_roll', ] diff --git a/src/wroboarm_22/config/kinematics.yaml b/src/wroboarm_22/config/kinematics.yaml new file mode 100644 index 00000000..05573e5c --- /dev/null +++ b/src/wroboarm_22/config/kinematics.yaml @@ -0,0 +1,4 @@ +arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/src/wroboarm_22/config/ompl_planning.yaml b/src/wroboarm_22/config/ompl_planning.yaml new file mode 100644 index 00000000..47312ca4 --- /dev/null +++ b/src/wroboarm_22/config/ompl_planning.yaml @@ -0,0 +1,155 @@ +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: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + 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: + 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: + 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: + 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: + 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: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + 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: + 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: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + 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: + 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: + type: geometric::PDST + STRIDE: + 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: + 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: + 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: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + 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: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + 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: + 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 +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 diff --git a/src/wroboarm_22/config/ros_controllers.yaml b/src/wroboarm_22/config/ros_controllers.yaml new file mode 100644 index 00000000..f8e4418a --- /dev/null +++ b/src/wroboarm_22/config/ros_controllers.yaml @@ -0,0 +1,35 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: todo_group_name + joint_model_group_pose: todo_state_name +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - turntable + - shoulder + - elbow + - forearm_roll + - wrist_pitch + - wrist_roll + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - turntable + - shoulder + - elbow + - forearm_roll + - wrist_pitch + - wrist_roll \ No newline at end of file diff --git a/src/wroboarm_22/config/sensors_3d.yaml b/src/wroboarm_22/config/sensors_3d.yaml new file mode 100644 index 00000000..a4bb13e3 --- /dev/null +++ b/src/wroboarm_22/config/sensors_3d.yaml @@ -0,0 +1,10 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater \ No newline at end of file diff --git a/src/wroboarm_22/config/simple_moveit_controllers.yaml b/src/wroboarm_22/config/simple_moveit_controllers.yaml new file mode 100644 index 00000000..044fd7a1 --- /dev/null +++ b/src/wroboarm_22/config/simple_moveit_controllers.yaml @@ -0,0 +1,23 @@ +controller_list: + - name: arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - turntable + - shoulder + - elbow + - forearm_roll + - wrist_pitch + - wrist_roll + - name: arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - turntable + - shoulder + - elbow + - forearm_roll + - wrist_pitch + - wrist_roll \ No newline at end of file diff --git a/src/wroboarm_22/config/wroboarm_22.srdf b/src/wroboarm_22/config/wroboarm_22.srdf new file mode 100644 index 00000000..97c46660 --- /dev/null +++ b/src/wroboarm_22/config/wroboarm_22.srdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/export.log b/src/wroboarm_22/export.log new file mode 100644 index 00000000..0f28f844 --- /dev/null +++ b/src/wroboarm_22/export.log @@ -0,0 +1,566 @@ +2022-01-28 23:47:18,488 INFO Logger.cs: 70 - +-------------------------------------------------------------------------------- +2022-01-28 23:47:18,508 INFO Logger.cs: 71 - Logging commencing for SW2URDF exporter +2022-01-28 23:47:18,508 INFO Logger.cs: 73 - Commit version 1.6.0-4-g7f85cfe +2022-01-28 23:47:18,509 INFO Logger.cs: 74 - Build version 1.6.7995.38578 +2022-01-28 23:47:18,510 INFO SwAddin.cs: 192 - Attempting to connect to SW +2022-01-28 23:47:18,511 INFO SwAddin.cs: 197 - Setting up callbacks +2022-01-28 23:47:18,511 INFO SwAddin.cs: 201 - Setting up command manager +2022-01-28 23:47:18,512 INFO SwAddin.cs: 204 - Adding command manager +2022-01-28 23:47:18,517 INFO SwAddin.cs: 263 - Adding Assembly export to file menu +2022-01-28 23:47:18,517 INFO SwAddin.cs: 272 - Adding Part export to file menu +2022-01-28 23:47:18,518 INFO SwAddin.cs: 210 - Adding event handlers +2022-01-28 23:47:18,520 INFO SwAddin.cs: 217 - Connecting plugin to SolidWorks +2022-01-29 04:01:04,989 INFO SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM +2022-01-29 04:01:07,687 INFO SwAddin.cs: 313 - Saving assembly +2022-01-29 04:01:07,864 INFO SwAddin.cs: 316 - Opening property manager +2022-01-29 04:01:07,891 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:01:07,894 INFO ExportHelperExtension.cs: 1136 - Found 44 in ArmAssembly.SLDASM +2022-01-29 04:01:07,894 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:01:07,895 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:07,895 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:07,930 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:01:07,930 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:07,931 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:01:07,931 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:01:07,931 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:07,932 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:01:07,932 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:01:07,932 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:07,933 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:01:07,933 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:01:07,934 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:07,934 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:01:07,934 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:01:07,935 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:07,936 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:01:07,936 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:01:07,936 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:07,937 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:01:07,937 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1136 - Found 44 in ArmAssembly.SLDASM +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:07,938 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:07,939 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:01:07,939 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:07,939 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:01:07,940 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:01:07,940 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:07,940 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:01:07,941 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:01:07,942 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:07,942 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:01:07,942 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:01:07,943 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:07,943 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:01:07,943 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:01:07,944 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:07,944 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:01:07,944 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:01:07,945 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:07,945 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:01:08,306 INFO SwAddin.cs: 339 - Loading config tree +2022-01-29 04:01:08,309 INFO ExportPropertyManagerExtension.cs: 520 - Starting new configuration +2022-01-29 04:01:08,315 INFO SwAddin.cs: 344 - Showing property manager +2022-01-29 04:01:36,796 INFO ExportPropertyManager.cs: 422 - Configuration saved +2022-01-29 04:01:38,889 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:01:38,895 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:38,895 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:01:38,895 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:38,896 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:38,896 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:01:38,896 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:38,897 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:01:38,897 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:01:38,897 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:38,898 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:01:38,898 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:01:38,899 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:38,899 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:01:38,899 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:01:38,900 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:38,900 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:01:38,900 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:01:38,901 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:38,901 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:01:38,902 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:01:38,902 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:38,902 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:01:38,903 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:01:38,903 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:38,903 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:01:38,904 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:01:38,904 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:38,904 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:01:38,905 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:38,905 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:38,905 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:01:38,906 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:38,906 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:01:38,906 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:01:38,907 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:38,908 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:01:38,908 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:01:38,908 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:38,909 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:01:38,909 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:01:38,909 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:38,910 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:01:38,910 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:01:38,910 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:38,911 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:01:38,911 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:01:38,911 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:38,912 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:01:38,958 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:01:48,808 INFO SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM +2022-01-29 04:01:48,808 INFO SwAddin.cs: 299 - Save is required +2022-01-29 04:01:48,809 INFO SwAddin.cs: 313 - Saving assembly +2022-01-29 04:01:49,233 INFO SwAddin.cs: 316 - Opening property manager +2022-01-29 04:01:49,234 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:01:49,235 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:49,235 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:01:49,235 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:49,236 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:49,236 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:01:49,236 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:49,237 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:01:49,237 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:01:49,237 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:49,238 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:01:49,238 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:01:49,238 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:49,239 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:01:49,239 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:01:49,240 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:49,240 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:01:49,240 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:01:49,241 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:49,241 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:01:49,241 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:01:49,242 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:49,248 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:01:49,248 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:01:49,279 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:49,279 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:01:49,280 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:01:49,281 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:01:49,281 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:01:49,282 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:01:49,282 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:01:49,282 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:01:49,283 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:01:49,283 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:01:49,283 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:01:49,284 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:01:49,284 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:01:49,285 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:01:49,285 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:01:49,285 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:01:49,286 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:01:49,286 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:01:49,287 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:01:49,288 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:01:49,288 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:01:49,288 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:01:49,398 INFO SwAddin.cs: 339 - Loading config tree +2022-01-29 04:01:49,400 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:01:49,445 INFO LinkNode.cs: 35 - Building node base_link +2022-01-29 04:01:49,446 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM +2022-01-29 04:01:49,447 INFO CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly +2022-01-29 04:01:49,448 INFO CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\Tests\URDF_Assembly\base.SLDPRT +2022-01-29 04:01:49,449 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link base_link +2022-01-29 04:01:49,471 INFO SwAddin.cs: 344 - Showing property manager +2022-01-29 04:02:15,278 INFO ExportPropertyManager.cs: 422 - Configuration saved +2022-01-29 04:02:15,279 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:02:15,425 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:02:15,426 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:02:15,426 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:02:15,427 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:02:15,427 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:02:15,427 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:02:15,428 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:02:15,428 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:02:15,428 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:02:15,429 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:02:15,429 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:02:15,429 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:02:15,430 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:02:15,431 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:02:15,431 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:02:15,431 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:02:15,432 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:02:15,432 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:02:15,432 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:02:15,433 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:02:15,433 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:02:15,433 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:02:15,434 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:02:15,434 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:02:15,434 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:02:15,435 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:02:15,435 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:02:15,435 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:02:15,436 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:02:15,436 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:02:15,436 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:02:15,437 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:02:15,437 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:02:15,437 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:02:15,438 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:02:15,438 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:02:15,438 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:02:15,439 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:02:15,439 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:02:15,439 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:02:15,440 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:02:15,440 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:02:15,441 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:02:15,442 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:02:15,442 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:02:15,443 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:02:15,488 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:03:44,437 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:03:50,608 INFO SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM +2022-01-29 04:03:50,608 INFO SwAddin.cs: 299 - Save is required +2022-01-29 04:03:50,608 INFO SwAddin.cs: 313 - Saving assembly +2022-01-29 04:03:50,733 INFO SwAddin.cs: 316 - Opening property manager +2022-01-29 04:03:50,733 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:03:50,734 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:03:50,734 INFO ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:03:50,734 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:03:50,735 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:03:50,735 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:03:50,736 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:03:50,736 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:03:50,736 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:03:50,737 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:03:50,737 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:03:50,738 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:03:50,738 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:03:50,739 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:03:50,739 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:03:50,740 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:03:50,740 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:03:50,740 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:03:50,741 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:03:50,741 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:03:50,741 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:03:50,742 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:03:50,743 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:03:50,743 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:03:50,743 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:03:50,744 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:03:50,744 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:03:50,745 INFO ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM +2022-01-29 04:03:50,745 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:03:50,745 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:03:50,746 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:03:50,746 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:03:50,746 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:03:50,747 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:03:50,747 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:03:50,747 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:03:50,748 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:03:50,748 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:03:50,748 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:03:50,749 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:03:50,749 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:03:50,749 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:03:50,750 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:03:50,750 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:03:50,750 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:03:50,751 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:03:50,751 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:03:50,752 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:03:50,752 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:03:50,752 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:03:50,753 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:03:50,753 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:03:50,802 INFO SwAddin.cs: 339 - Loading config tree +2022-01-29 04:03:50,803 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue9.1830074983591922E-190.0156064900891439551.5715252581589567E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504422ixytrue8.02209898558041E-21ixztrue-7.374852413660824E-20iyytrue0.0044229329573873041iyztrue2.2568808327940613E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:03:50,803 INFO LinkNode.cs: 35 - Building node base_link +2022-01-29 04:03:50,804 INFO CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM +2022-01-29 04:03:50,804 INFO CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly +2022-01-29 04:03:50,804 INFO CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\Tests\URDF_Assembly\base.SLDPRT +2022-01-29 04:03:50,805 INFO CommonSwOperations.cs: 230 - Loaded 1 components for link base_link +2022-01-29 04:03:50,813 INFO SwAddin.cs: 344 - Showing property manager +2022-01-29 04:03:55,199 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:04:02,915 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:04:06,718 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:04:08,432 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:07:25,776 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:07:36,624 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:08:34,715 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:09:06,042 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:09:28,401 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:09:33,716 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:10:26,057 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:10:32,832 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:01,760 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:10,262 INFO ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:32,320 INFO ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:11:50,721 INFO ExportPropertyManager.cs: 422 - Configuration saved +2022-01-29 04:11:50,723 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue9.1830074983591922E-190.0156064900891439551.5715252581589567E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504422ixytrue8.02209898558041E-21ixztrue-7.374852413660824E-20iyytrue0.0044229329573873041iyztrue2.2568808327940613E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:11:50,809 INFO ExportHelperExtension.cs: 347 - Creating joint shoulderBase_link +2022-01-29 04:11:50,815 INFO ExportHelperExtension.cs: 1397 - Fixing components for base_link +2022-01-29 04:11:50,816 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:50,816 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:51,179 INFO : 0 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:51,183 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:51,183 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:51,184 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:53,156 WARN ExportHelperExtension.cs: 351 - Creating joint from parent base_link to child shoulderBase_link failed +2022-01-29 04:11:53,162 INFO ExportHelperExtension.cs: 347 - Creating joint upperArm_link +2022-01-29 04:11:53,163 INFO ExportHelperExtension.cs: 1397 - Fixing components for shoulderBase_link +2022-01-29 04:11:53,163 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:53,163 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:53,164 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:53,382 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:53,383 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:53,383 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:53,383 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:53,384 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:11:55,357 WARN ExportHelperExtension.cs: 351 - Creating joint from parent shoulderBase_link to child upperArm_link failed +2022-01-29 04:11:55,362 INFO ExportHelperExtension.cs: 347 - Creating joint forarm_link +2022-01-29 04:11:55,362 INFO ExportHelperExtension.cs: 1397 - Fixing components for upperArm_link +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:55,363 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:55,607 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:55,607 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:55,607 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:55,608 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:55,608 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:11:55,608 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:11:57,637 WARN ExportHelperExtension.cs: 351 - Creating joint from parent upperArm_link to child forarm_link failed +2022-01-29 04:11:57,644 INFO ExportHelperExtension.cs: 347 - Creating joint wirstBase_link +2022-01-29 04:11:57,644 INFO ExportHelperExtension.cs: 1397 - Fixing components for forarm_link +2022-01-29 04:11:57,645 INFO ExportHelperExtension.cs: 1402 - Fixing 35 +2022-01-29 04:11:57,645 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:11:57,645 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:57,646 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:57,646 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:11:57,884 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:11:57,885 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:11:57,885 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:11:57,886 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:11:57,886 INFO ExportHelperExtension.cs: 1352 - Unfixing component 35 +2022-01-29 04:11:57,886 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:11:57,887 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:11:59,971 WARN ExportHelperExtension.cs: 351 - Creating joint from parent forarm_link to child wirstBase_link failed +2022-01-29 04:11:59,975 INFO ExportHelperExtension.cs: 347 - Creating joint wrist_link +2022-01-29 04:11:59,975 INFO ExportHelperExtension.cs: 1397 - Fixing components for wirstBase_link +2022-01-29 04:11:59,976 INFO ExportHelperExtension.cs: 1402 - Fixing 40 +2022-01-29 04:11:59,976 INFO ExportHelperExtension.cs: 1402 - Fixing 35 +2022-01-29 04:11:59,976 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:11:59,977 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:11:59,977 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:11:59,977 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:12:00,240 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:12:00,241 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:12:00,241 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:12:00,241 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:12:00,242 INFO ExportHelperExtension.cs: 1352 - Unfixing component 40 +2022-01-29 04:12:00,242 INFO ExportHelperExtension.cs: 1352 - Unfixing component 35 +2022-01-29 04:12:00,242 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:12:00,243 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:12:02,368 WARN ExportHelperExtension.cs: 351 - Creating joint from parent wirstBase_link to child wrist_link failed +2022-01-29 04:12:02,372 INFO ExportHelperExtension.cs: 347 - Creating joint manipulator_link +2022-01-29 04:12:02,373 INFO ExportHelperExtension.cs: 1397 - Fixing components for wrist_link +2022-01-29 04:12:02,373 INFO ExportHelperExtension.cs: 1402 - Fixing 39 +2022-01-29 04:12:02,373 INFO ExportHelperExtension.cs: 1402 - Fixing 40 +2022-01-29 04:12:02,374 INFO ExportHelperExtension.cs: 1402 - Fixing 35 +2022-01-29 04:12:02,374 INFO ExportHelperExtension.cs: 1402 - Fixing 26 +2022-01-29 04:12:02,374 INFO ExportHelperExtension.cs: 1402 - Fixing 25 +2022-01-29 04:12:02,375 INFO ExportHelperExtension.cs: 1402 - Fixing 24 +2022-01-29 04:12:02,375 INFO ExportHelperExtension.cs: 1409 - Component 24 is already fixed +2022-01-29 04:12:02,655 INFO ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[] +2022-01-29 04:12:02,655 INFO ExportHelperExtension.cs: 841 - R2: 0, 0 +2022-01-29 04:12:02,656 INFO ExportHelperExtension.cs: 849 - L1: 0 +2022-01-29 04:12:02,656 INFO ExportHelperExtension.cs: 857 - L2: 0 +2022-01-29 04:12:02,698 INFO ExportHelperExtension.cs: 1352 - Unfixing component 39 +2022-01-29 04:12:02,699 INFO ExportHelperExtension.cs: 1352 - Unfixing component 40 +2022-01-29 04:12:02,699 INFO ExportHelperExtension.cs: 1352 - Unfixing component 35 +2022-01-29 04:12:02,699 INFO ExportHelperExtension.cs: 1352 - Unfixing component 26 +2022-01-29 04:12:02,700 INFO ExportHelperExtension.cs: 1352 - Unfixing component 25 +2022-01-29 04:12:04,927 WARN ExportHelperExtension.cs: 351 - Creating joint from parent wrist_link to child manipulator_link failed +2022-01-29 04:12:05,029 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM +2022-01-29 04:12:05,030 INFO ExportHelperExtension.cs: 1136 - Found 59 in ArmAssembly.SLDASM +2022-01-29 04:12:05,031 INFO ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in ArmAssembly.SLDASM +2022-01-29 04:12:05,031 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:12:05,031 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:12:05,032 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1 +2022-01-29 04:12:05,032 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:12:05,032 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1 +2022-01-29 04:12:05,033 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1 +2022-01-29 04:12:05,033 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:12:05,033 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1 +2022-01-29 04:12:05,034 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1 +2022-01-29 04:12:05,034 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:12:05,035 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1 +2022-01-29 04:12:05,035 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1 +2022-01-29 04:12:05,035 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:12:05,036 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1 +2022-01-29 04:12:05,036 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1 +2022-01-29 04:12:05,036 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:12:05,037 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1 +2022-01-29 04:12:05,037 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1 +2022-01-29 04:12:05,037 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:12:05,038 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1 +2022-01-29 04:12:05,038 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1 +2022-01-29 04:12:05,038 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:12:05,039 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1 +2022-01-29 04:12:05,039 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM +2022-01-29 04:12:05,039 INFO ExportHelperExtension.cs: 1136 - Found 59 in ArmAssembly.SLDASM +2022-01-29 04:12:05,040 INFO ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in ArmAssembly.SLDASM +2022-01-29 04:12:05,040 INFO ExportHelperExtension.cs: 1148 - Proceeding through assembly components +2022-01-29 04:12:05,040 INFO ExportHelperExtension.cs: 1160 - 7 components to check +2022-01-29 04:12:05,041 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1 +2022-01-29 04:12:05,041 INFO ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1 +2022-01-29 04:12:05,041 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1 +2022-01-29 04:12:05,042 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1 +2022-01-29 04:12:05,042 INFO ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1 +2022-01-29 04:12:05,042 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1 +2022-01-29 04:12:05,043 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1 +2022-01-29 04:12:05,044 INFO ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1 +2022-01-29 04:12:05,044 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1 +2022-01-29 04:12:05,044 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1 +2022-01-29 04:12:05,045 INFO ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1 +2022-01-29 04:12:05,045 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1 +2022-01-29 04:12:05,045 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1 +2022-01-29 04:12:05,046 INFO ExportHelperExtension.cs: 1136 - Found 27 in base-1 +2022-01-29 04:12:05,046 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1 +2022-01-29 04:12:05,046 INFO ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1 +2022-01-29 04:12:05,047 INFO ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1 +2022-01-29 04:12:05,047 INFO ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1 +2022-01-29 04:12:05,103 INFO ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message +2022-01-29 04:13:17,527 INFO AssemblyExportForm.cs: 253 - Completing URDF export +2022-01-29 04:13:17,529 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue9.1830074983591922E-190.0156064900891439551.5715252581589567E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504422ixytrue8.02209898558041E-21ixztrue-7.374852413660824E-20iyytrue0.0044229329573873041iyztrue2.2568808327940613E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GeneratelinktruenametrueshoulderBase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueturntabletypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically GeneratelinktruenametrueupperArm_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueshouldertypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametrueforarm_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueelbowtypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically GeneratelinktruenametruewirstBase_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueforearm_rolltypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametruewrist_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_pitchtypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically Generatelinktruenametruemanipulator_linkxyztrue000rpytrue000originfalsefalsevaluetrue0massfalseixxtrue0ixytrue0ixztrue0iyytrue0iyztrue0izztrue0inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue1111colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_rolltypetruerevolutexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAutomatically GenerateAutomatically GeneratelinktruefalsesDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==falsefalsefalsesDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==falsefalsefalsesDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==falsefalsefalsesDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAAfalsefalsefalsesDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==falsefalsefalsesDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==falsefalsefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:13:41,728 INFO AssemblyExportForm.cs: 309 - Saving URDF package to E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM +2022-01-29 04:13:41,730 INFO ExportHelper.cs: 147 - Beginning the export process +2022-01-29 04:13:41,731 INFO ExportHelper.cs: 153 - Creating package directories with name ArmAssembly.SLDASM and save path E:\Robotics\WR\Tests\URDF_Assembly +2022-01-29 04:13:46,142 ERROR AssemblyExportForm.cs: 126 - Exception encountered in Assembly export form +System.IO.IOException: Cannot create "E:\Robotics\WR\Tests\URDF_Assembly\ArmAssembly.SLDASM" because a file or directory with the same name already exists. + at System.IO.__Error.WinIOError(Int32 errorCode, String maybeFullPath) + at System.IO.Directory.InternalCreateDirectory(String fullPath, String path, Object dirSecurityObj, Boolean checkHost) + at System.IO.Directory.InternalCreateDirectoryHelper(String path, Boolean checkHost) + at SW2URDF.URDFExport.URDFPackage.CreateDirectories() in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\URDFExport\URDFPackage.cs:line 78 + at SW2URDF.URDFExport.ExportHelper.ExportRobot(Boolean exportSTL) in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\URDFExport\ExportHelper.cs:line 155 + at SW2URDF.UI.AssemblyExportForm.FinishExport(Boolean exportSTL) in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\UI\AssemblyExportForm.cs:line 310 + at SW2URDF.UI.AssemblyExportForm.ButtonLinksFinishClick(Object sender, EventArgs e) in C:\Users\Stephen Brawner\workspace\solidworks_urdf_exporter\SW2URDF\UI\AssemblyExportForm.cs:line 243 + at System.Windows.Forms.Control.OnClick(EventArgs e) + at System.Windows.Forms.Button.OnClick(EventArgs e) + at System.Windows.Forms.Button.OnMouseUp(MouseEventArgs mevent) + at System.Windows.Forms.Control.WmMouseUp(Message& m, MouseButtons button, Int32 clicks) + at System.Windows.Forms.Control.WndProc(Message& m) + at System.Windows.Forms.ButtonBase.WndProc(Message& m) + at System.Windows.Forms.Button.WndProc(Message& m) + at System.Windows.Forms.NativeWindow.Callback(IntPtr hWnd, Int32 msg, IntPtr wparam, IntPtr lparam) +2022-01-29 04:13:53,196 INFO AssemblyExportForm.cs: 253 - Completing URDF export +2022-01-29 04:13:53,197 INFO ConfigurationSerialization.cs: 276 - URDF Configuration found +nametruebase_linkxyztrue1.035828411313638E-180.0156064900891439571.9357498027169635E-18rpytrue000originfalsefalsevaluetrue0.93972387307473026massfalseixxtrue0.0025142902225504418ixytrue8.6586155170563642E-21ixztrue-7.3748524136608228E-20iyytrue0.0044229329573873041iyztrue2.2771129801693437E-20izztrue0.0021336882069818023inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruetypetruexyztrue000rpytrue000originfalsefalselinktrueparenttruelinktruechildtruexyztrue000axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseOrigin_globallinktruenametrueshoulderBase_linkxyztrue-0.0122586821821900790.1285772495779757-0.0037054435004292684rpytrue000originfalsefalsevaluetrue0.99636680500272734massfalseixxtrue0.0065237009420894719ixytrue0.0010646616036528998ixztrue0.00028312700527736404iyytrue0.0050231810993567032iyztrue6.82113719161566E-05izztrue0.0086334538259584inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueturntabletypetruecontinuousxyztrue000.035865rpytrue1.57080-0.25129originfalsefalselinktruebase_linkparenttruelinktrueshoulderBase_linkchildtruexyztrue010axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_turntableOrigin_turntablelinktruenametrueupperArm_linkxyztrue0.014397399905456463-3.2006162568792851E-050.27934838479492807rpytrue000originfalsefalsevaluetrue0.98521034391581119massfalseixxtrue0.023798222473833903ixytrue-8.87977733299251E-07ixztrue-0.0038853285623011904iyytrue0.029359472259760169iyztrue-7.1294436997127461E-06izztrue0.0062503483932010362inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueshouldertypetruecontinuousxyztrue00.180980.043071rpytrue-1.102100originfalsefalselinktrueshoulderBase_linkparenttruelinktrueupperArm_linkchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_shoulderOrigin_shoulderlinktruenametrueforarm_linkxyztrue1.8457457784393227E-150.0550154986611603450.12015280303424725rpytrue000originfalsefalsevaluetrue0.51779610173045887massfalseixxtrue0.00404850874710771ixytrue2.1141942363467336E-18ixztrue-1.5487150407597627E-17iyytrue0.003755483293922739iyztrue-0.00037738536370652919izztrue0.00078832783822656164inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueelbowtypetruecontinuousxyztrue000.46634rpytrue2.493600originfalsefalselinktrueupperArm_linkparenttruelinktrueforarm_linkchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_elbowOrigin_elbowlinktruenametruewirstBase_linkxyztrue-1.9428902930940239E-16-1.6653345369377348E-150.09870348455642336rpytrue000originfalsefalsevaluetrue0.30176308217533721massfalseixxtrue0.0021034428970283731ixytrue-2.7105054312137611E-19ixztrue-2.2497195079074217E-18iyytrue0.003050162507065583iyztrue1.8566962203814263E-18izztrue0.0010760910650178595inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametrueforearm_rolltypetruecontinuousxyztrue00.044450.2285rpytrue000originfalsefalselinktrueforarm_linkparenttruelinktruewirstBase_linkchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_forearm_rollOrigin_forearm_rolllinktruenametruewrist_linkxyztrue4.3021142204224816E-16-2.7755575615628914E-170rpytrue000originfalsefalsevaluetrue0.44770711794317625massfalseixxtrue0.00036770226690479489ixytrue1.7999450129153882E-19ixztrue-7.453889935837843E-19iyytrue0.00084342708626575322iyztrue-1.3552527156068805E-20izztrue0.00084474612356065992inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_pitchtypetruecontinuousxyztrue000.23716rpytrue-1.33300originfalsefalselinktruewirstBase_linkparenttruelinktruewrist_linkchildtruexyztrue100axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_wrist_pitchOrigin_wrist_pitchlinktruenametruemanipulator_linkxyztrue-0.00073032374046050341-0.000899365669252000230.049056009995342265rpytrue000originfalsefalsevaluetrue0.39742022555481077massfalseixxtrue0.0015968430526387824ixytrue-3.0724322789165625E-06ixztrue2.5323557993615128E-05iyytrue0.0016343224414687998iyztrue5.0029229087463509E-05izztrue0.00025632256399428705inertiafalseinertialfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruenametruergbatrue0.7921568627450980.819607843137254880.933333333333333351colorfalsefilenametruetexturefalsematerialfalsevisualfalsexyztrue000rpytrue000originfalsefalsefilenametruemeshfalsegeometrytruecollisionfalsenametruewrist_rolltypetruecontinuousxyztrue000rpytrue000.096862originfalsefalselinktruewrist_linkparenttruelinktruemanipulator_linkchildtruexyztrue001axisfalselowerfalseupperfalseefforttruevelocitytruelimitfalserisingfalsefallingfalsecalibrationfalsedampingfalsefrictionfalsedynamicsfalsesoft_upper_limitfalsesoft_lower_limitfalsek_positionfalsek_velocitytruesafety_controllerfalsejointtruemultiplierfalseoffsetfalsemimicfalsejointfalseAxis_wrist_rollOrigin_wrist_rolllinktruefalsesDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==falsefalsefalsesDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==falsefalsefalsesDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==falsefalsefalsesDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAAfalsefalsefalsesDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==falsefalsefalsesDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==falsefalsefalsesDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=falsefalse +2022-01-29 04:14:02,877 INFO AssemblyExportForm.cs: 309 - Saving URDF package to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM +2022-01-29 04:14:02,877 INFO ExportHelper.cs: 147 - Beginning the export process +2022-01-29 04:14:02,877 INFO ExportHelper.cs: 153 - Creating package directories with name ArmAssembly.SLDASM and save path E:\Robotics\WR\Tests\URDF_Assembly\URDF +2022-01-29 04:14:04,383 INFO ExportHelper.cs: 162 - Creating CMakeLists.txt at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\CMakeLists.txt +2022-01-29 04:14:04,384 INFO ExportHelper.cs: 166 - Creating joint names config at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\config\joint_names_ArmAssembly.SLDASM.yaml +2022-01-29 04:14:04,385 INFO ExportHelper.cs: 170 - Creating package.xml at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\package.xml +2022-01-29 04:14:04,385 INFO PackageXMLWriter.cs: 21 - Creating package.xml at E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\package.xml +2022-01-29 04:14:04,389 INFO ExportHelper.cs: 177 - Creating RVIZ launch file in E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\launch\ +2022-01-29 04:14:04,391 INFO ExportHelper.cs: 182 - Creating Gazebo launch file in E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\launch\ +2022-01-29 04:14:04,392 INFO ExportHelper.cs: 187 - Saving existing STL preferences +2022-01-29 04:14:04,392 INFO ExportHelper.cs: 450 - Saving users preferences +2022-01-29 04:14:04,393 INFO ExportHelper.cs: 190 - Modifying STL preferences +2022-01-29 04:14:04,394 INFO ExportHelper.cs: 464 - Setting STL preferences +2022-01-29 04:14:04,397 INFO ExportHelper.cs: 196 - Found 0 hidden components +2022-01-29 04:14:04,398 INFO ExportHelper.cs: 197 - Hiding all components +2022-01-29 04:14:04,618 INFO ExportHelper.cs: 204 - Beginning individual files export +2022-01-29 04:14:04,622 INFO ExportHelper.cs: 270 - Exporting link: base_link +2022-01-29 04:14:04,623 INFO ExportHelper.cs: 272 - Link base_link has 1 children +2022-01-29 04:14:04,624 INFO ExportHelper.cs: 270 - Exporting link: shoulderBase_link +2022-01-29 04:14:04,624 INFO ExportHelper.cs: 272 - Link shoulderBase_link has 1 children +2022-01-29 04:14:04,625 INFO ExportHelper.cs: 270 - Exporting link: upperArm_link +2022-01-29 04:14:04,625 INFO ExportHelper.cs: 272 - Link upperArm_link has 1 children +2022-01-29 04:14:04,625 INFO ExportHelper.cs: 270 - Exporting link: forarm_link +2022-01-29 04:14:04,626 INFO ExportHelper.cs: 272 - Link forarm_link has 1 children +2022-01-29 04:14:04,626 INFO ExportHelper.cs: 270 - Exporting link: wirstBase_link +2022-01-29 04:14:04,627 INFO ExportHelper.cs: 272 - Link wirstBase_link has 1 children +2022-01-29 04:14:04,628 INFO ExportHelper.cs: 270 - Exporting link: wrist_link +2022-01-29 04:14:04,628 INFO ExportHelper.cs: 272 - Link wrist_link has 1 children +2022-01-29 04:14:04,629 INFO ExportHelper.cs: 270 - Exporting link: manipulator_link +2022-01-29 04:14:04,629 INFO ExportHelper.cs: 272 - Link manipulator_link has 0 children +2022-01-29 04:14:04,630 INFO ExportHelper.cs: 317 - manipulator_link: Exporting STL with coordinate frame Origin_wrist_roll +2022-01-29 04:14:04,631 INFO ExportHelper.cs: 322 - manipulator_link: Reference geometry name +2022-01-29 04:14:04,669 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\manipulator_link.STL +2022-01-29 04:14:04,785 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,785 INFO ExportHelper.cs: 317 - wrist_link: Exporting STL with coordinate frame Origin_wrist_pitch +2022-01-29 04:14:04,786 INFO ExportHelper.cs: 322 - wrist_link: Reference geometry name +2022-01-29 04:14:04,797 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\wrist_link.STL +2022-01-29 04:14:04,834 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,835 INFO ExportHelper.cs: 317 - wirstBase_link: Exporting STL with coordinate frame Origin_forearm_roll +2022-01-29 04:14:04,835 INFO ExportHelper.cs: 322 - wirstBase_link: Reference geometry name +2022-01-29 04:14:04,846 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\wirstBase_link.STL +2022-01-29 04:14:04,873 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,874 INFO ExportHelper.cs: 317 - forarm_link: Exporting STL with coordinate frame Origin_elbow +2022-01-29 04:14:04,874 INFO ExportHelper.cs: 322 - forarm_link: Reference geometry name +2022-01-29 04:14:04,932 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\forarm_link.STL +2022-01-29 04:14:04,986 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:04,987 INFO ExportHelper.cs: 317 - upperArm_link: Exporting STL with coordinate frame Origin_shoulder +2022-01-29 04:14:04,987 INFO ExportHelper.cs: 322 - upperArm_link: Reference geometry name +2022-01-29 04:14:05,001 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\upperArm_link.STL +2022-01-29 04:14:05,035 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:05,035 INFO ExportHelper.cs: 317 - shoulderBase_link: Exporting STL with coordinate frame Origin_turntable +2022-01-29 04:14:05,036 INFO ExportHelper.cs: 322 - shoulderBase_link: Reference geometry name +2022-01-29 04:14:05,111 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\shoulderBase_link.STL +2022-01-29 04:14:05,189 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:05,190 INFO ExportHelper.cs: 317 - base_link: Exporting STL with coordinate frame Origin_global +2022-01-29 04:14:05,191 INFO ExportHelper.cs: 322 - base_link: Reference geometry name +2022-01-29 04:14:05,235 INFO ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\meshes\base_link.STL +2022-01-29 04:14:05,274 INFO ExportHelper.cs: 405 - Removing SW header in STL file +2022-01-29 04:14:05,275 INFO ExportHelper.cs: 145 - Showing all components except previously hidden components +2022-01-29 04:14:05,465 INFO ExportHelper.cs: 145 - Resetting STL preferences +2022-01-29 04:14:05,466 INFO ExportHelper.cs: 478 - Returning STL preferences to user preferences +2022-01-29 04:14:05,466 INFO ExportHelper.cs: 228 - Writing URDF file to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\urdf\ArmAssembly.SLDASM.urdf +2022-01-29 04:14:05,470 INFO CSVImportExport.cs: 32 - Writing CSV file E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\urdf\ArmAssembly.SLDASM.csv +2022-01-29 04:14:05,493 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,494 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,495 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,495 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,495 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,496 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,496 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link, +2022-01-29 04:14:05,497 INFO ExportHelper.cs: 234 - Copying log file +2022-01-29 04:14:05,497 INFO ExportHelper.cs: 439 - Copying C:\Users\Payton Jackson\sw2urdf_logs\sw2urdf.log to E:\Robotics\WR\Tests\URDF_Assembly\URDF\ArmAssembly.SLDASM\export.log diff --git a/src/wroboarm_22/launch/arm_assembly_moveit_sensor_manager.launch.xml b/src/wroboarm_22/launch/arm_assembly_moveit_sensor_manager.launch.xml new file mode 100644 index 00000000..5d02698d --- /dev/null +++ b/src/wroboarm_22/launch/arm_assembly_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/src/wroboarm_22/launch/chomp_planning_pipeline.launch.xml b/src/wroboarm_22/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..97a44bff --- /dev/null +++ b/src/wroboarm_22/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/default_warehouse_db.launch b/src/wroboarm_22/launch/default_warehouse_db.launch new file mode 100644 index 00000000..198a981d --- /dev/null +++ b/src/wroboarm_22/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/demo.launch b/src/wroboarm_22/launch/demo.launch new file mode 100644 index 00000000..fb9b64ca --- /dev/null +++ b/src/wroboarm_22/launch/demo.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/demo_gazebo.launch b/src/wroboarm_22/launch/demo_gazebo.launch new file mode 100644 index 00000000..b8005362 --- /dev/null +++ b/src/wroboarm_22/launch/demo_gazebo.launch @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/demo_test.launch b/src/wroboarm_22/launch/demo_test.launch new file mode 100644 index 00000000..ff35c359 --- /dev/null +++ b/src/wroboarm_22/launch/demo_test.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [control/arm_joint_states] + + + [control/arm_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/display.launch b/src/wroboarm_22/launch/display.launch new file mode 100644 index 00000000..21a998d4 --- /dev/null +++ b/src/wroboarm_22/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + \ No newline at end of file diff --git a/src/wroboarm_22/launch/fake_moveit_controller_manager.launch.xml b/src/wroboarm_22/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..dc8b90e9 --- /dev/null +++ b/src/wroboarm_22/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/gazebo copy.launch b/src/wroboarm_22/launch/gazebo copy.launch new file mode 100644 index 00000000..bc973f34 --- /dev/null +++ b/src/wroboarm_22/launch/gazebo copy.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/gazebo.launch b/src/wroboarm_22/launch/gazebo.launch new file mode 100644 index 00000000..d74221c0 --- /dev/null +++ b/src/wroboarm_22/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + \ No newline at end of file diff --git a/src/wroboarm_22/launch/joystick_control.launch b/src/wroboarm_22/launch/joystick_control.launch new file mode 100644 index 00000000..9411f6e6 --- /dev/null +++ b/src/wroboarm_22/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/move_group.launch b/src/wroboarm_22/launch/move_group.launch new file mode 100644 index 00000000..ccea67e0 --- /dev/null +++ b/src/wroboarm_22/launch/move_group.launch @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/moveit.rviz b/src/wroboarm_22/launch/moveit.rviz new file mode 100644 index 00000000..697a23e2 --- /dev/null +++ b/src/wroboarm_22/launch/moveit.rviz @@ -0,0 +1,338 @@ +Panels: + - Class: rviz/Displays + Help Height: 107 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /Pose1 + - /TF1/Frames1 + - /MarkerArray1 + - /PointCloud21 + Splitter Ratio: 0.47119078040122986 + Tree Height: 360 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + forarm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + manipulator_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulderBase_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + upperArm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wirstBase_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 1 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: false + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + forarm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + manipulator_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulderBase_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + upperArm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wirstBase_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.05000000074505806 + Name: Pose + Queue Size: 10 + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.029999999329447746 + Shape: Axes + Topic: /logic/arm_teleop/next_target + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: false + forarm_link: + Value: true + manipulator_link: + Value: true + shoulderBase_link: + Value: true + upperArm_link: + Value: true + wirstBase_link: + Value: true + wrist_link: + Value: true + Marker Alpha: 1 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + shoulderBase_link: + upperArm_link: + forarm_link: + wirstBase_link: + wrist_link: + manipulator_link: + {} + Update Interval: 0 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rviz_visual_tools + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /move_group/filtered_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 3.7679052352905273 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.48979657888412476 + Target Frame: base_link + Yaw: 5.20182991027832 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd00000001000000000000030f0000039efc0200000007fb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072000000020c000000410000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00ffffff000004330000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1864 + X: 56 + Y: 27 diff --git a/src/wroboarm_22/launch/moveit_rviz.launch b/src/wroboarm_22/launch/moveit_rviz.launch new file mode 100644 index 00000000..a4605c03 --- /dev/null +++ b/src/wroboarm_22/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/ompl_planning_pipeline.launch.xml b/src/wroboarm_22/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 00000000..f6aa6b50 --- /dev/null +++ b/src/wroboarm_22/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/src/wroboarm_22/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 00000000..9478782e --- /dev/null +++ b/src/wroboarm_22/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/planning_context.launch b/src/wroboarm_22/launch/planning_context.launch new file mode 100644 index 00000000..9e6bc4cb --- /dev/null +++ b/src/wroboarm_22/launch/planning_context.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/planning_pipeline.launch.xml b/src/wroboarm_22/launch/planning_pipeline.launch.xml new file mode 100644 index 00000000..93a9ab28 --- /dev/null +++ b/src/wroboarm_22/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/wroboarm_22/launch/ros_control_moveit_controller_manager.launch.xml b/src/wroboarm_22/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..9ebc91c1 --- /dev/null +++ b/src/wroboarm_22/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/wroboarm_22/launch/ros_controllers.launch b/src/wroboarm_22/launch/ros_controllers.launch new file mode 100644 index 00000000..da2ca750 --- /dev/null +++ b/src/wroboarm_22/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/src/wroboarm_22/launch/run_benchmark_ompl.launch b/src/wroboarm_22/launch/run_benchmark_ompl.launch new file mode 100644 index 00000000..8773b211 --- /dev/null +++ b/src/wroboarm_22/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/sensor_manager.launch.xml b/src/wroboarm_22/launch/sensor_manager.launch.xml new file mode 100644 index 00000000..bd976670 --- /dev/null +++ b/src/wroboarm_22/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/setup_assistant.launch b/src/wroboarm_22/launch/setup_assistant.launch new file mode 100644 index 00000000..41910f78 --- /dev/null +++ b/src/wroboarm_22/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/simple_moveit_controller_manager.launch.xml b/src/wroboarm_22/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..c8236948 --- /dev/null +++ b/src/wroboarm_22/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/wroboarm_22/launch/trajectory_execution.launch.xml b/src/wroboarm_22/launch/trajectory_execution.launch.xml new file mode 100644 index 00000000..4c9ce3ff --- /dev/null +++ b/src/wroboarm_22/launch/trajectory_execution.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/warehouse.launch b/src/wroboarm_22/launch/warehouse.launch new file mode 100644 index 00000000..d46aaeb6 --- /dev/null +++ b/src/wroboarm_22/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/warehouse_settings.launch.xml b/src/wroboarm_22/launch/warehouse_settings.launch.xml new file mode 100644 index 00000000..e473b083 --- /dev/null +++ b/src/wroboarm_22/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/wroboarm_22/launch/wroboarm_22_moveit_controller_manager.launch.xml b/src/wroboarm_22/launch/wroboarm_22_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..6e73a267 --- /dev/null +++ b/src/wroboarm_22/launch/wroboarm_22_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/wroboarm_22/launch/wroboarm_22_moveit_sensor_manager.launch.xml b/src/wroboarm_22/launch/wroboarm_22_moveit_sensor_manager.launch.xml new file mode 100644 index 00000000..5d02698d --- /dev/null +++ b/src/wroboarm_22/launch/wroboarm_22_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/src/wroboarm_22/meshes/base_link.STL b/src/wroboarm_22/meshes/base_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..c298f6b589bed4d5547bebeb7d6d33a87c54cc6a GIT binary patch literal 11984 zcmb_hZHyjO8J#R7f~8vpexM12?E6wQYSI8kC_C>662%6M8d6AVqX{IG$|jbExRywz zu24%G(5%LfDzrO@3urV_qs`7cKxlw8SS++gBZd$(TKpgqu_k^!ANS0AX6Ig(AHIKj zy3aY!nYka&y?1Vw{eORdk#xVf^1a2LSKe0aU$?rPKkezf9^akIzZU~PHu2!zSHH3^ z|LE%d`Dq)U+M8A|3s^H{*$F50i)-$2}@6qkK!@avVIEzZ3DOC?8uHSa?!@0r}PH5=?+SS}tZ z1+#$N<1KIfPChlcwdg&zv*ZY{=AGHRqFi&z*xPgu)c@1TQ%wrX{)G$S-|cgJvc<4a|BrXL;AlOjjNP`S-|cgBeBNV z2}@NG+*ROE?0o^IVvf))=iT zR!gD^W&yiLmVLfP6%3I|Bh13z#bZLbU4=#>iuP00QWN{gCI7;sQgwKzO5RC4!kc## z%mUU-R^hZiYN?5RjsR=k&^MLzjd+Cq=qQ*4>>gp1)KU}s90At6VT@HW#(Gv0btHBa z%mQ|gFxqRWiG7X$Yu+$dDw!+dky0=V*ge9mt)(XRIRdPC!<-+=oEMLjf?2@s5voru zHL=eTV9h(T`Ocoy#G%wK@kl9{1>AUO-8B!60Bc_H)7l>1Bh=lFf?2>#Tj}~?ZN(8_ zeGh3jU86#Og!QANU>2}@NDt~dXnoERVC@g-e_dXvdX{_V4df})P0xLm1cj{Q7{YG zJ*2`FR1+8P;ZPs#?_PIf3XO1HV6Ao``>I`iRUW-?Z+`k$pI=b-^X{L`56{1YJq))M z$hBB;^1`k8lh1x~2?evzZknm>!4X!BU3_I;|Mj+r2P>F`{_lFon^$=AnPc-+Z`1p% z6@;m)TEv4RtXMYvKy||px9OU)=sj4$tnl|dsjvtu#-I6ab@K~*Cc6rLLwSkM<(~}D2%f*MlRhxP8ai*aD)}_`qK^7JGV?Pple{?^;5^tT3h{3XZVilneg5 z@Z$QrmryV(%*}{G`XEQINpI#@0i>Urok6Dun+PjpEyRHI;4@Tlb9?y9B=i?Bl0 z#2l+=^r=k2EZ%*z72GD=R-sl7x=+OsR$TDPtqb#aelOCxtY8+tYjiz0!U}zMSq&l{ ztYDUS>)9aw9<2!Sir-}NJsJhGqz(0K5P2$&utM5xGCp=$!7P~tdNzo7aD){ySMvBA zWCgQiHrw~aZhvru6;fmDD`!{15mrc*$dlEl$;B+m-OQWz*yRW-q}t`lD%#{?7SI32 zqtve;s6P5V33EliUt#y*=UUUO+GPrkup*4aa7txGXk9y#g}$*fAgkL?#gRn{I;FA) zD+D*J*Y{Mc4m}%AckICtR$%_?x5f5*u!32b&HBZ&t>8#_cKfEwnc51Dup+cV`0D%` z?_qOW`yBH|$3oj9JiGO6cys$wAl|z73AX@?JemkA!gvec09e5uoR`O2`>8m>3ar}t z{W7g!R(Pts`{tN~d=l2vZ)o-So7+4OUp8%pBSm^#S+0=RSg1<+ z!1iS`v|IeCv1I;hPNiIY(I09wk42@zVUjzOD7f-yHAZ>~#G1 zr{*~0C|~od2xDikCcL0!0xf>x;NI>e*CI> z`_8F8M}RdiPA~3}QZNhHJ;vr&)%U*tqWX&USM)gota)*IagUUOS-|eGV*5wxZ`^Wv zea^vNpCiDU7v~-KNGX^F>>lNF8|pPbdUiPetz$io0Bc^Hque80!jcw)MKdgG?y z8RvYl#}Q!7i?2q(BcWgxuzTp-$bNTeb@e~jPH+TR^Wv+Kd!!W10(Os=ul-#8@BMEc z+%r6Tjw8UD7bi>iNGX^F>>jf(ZK^+g(|v=t|Kz8090At6IDfiFO2I5(_c;3F_4Sxs zAB?S9UUCFj^WyyJ9w`O0fZgNJ^dI17k zSe*ar`jR8SnpbwRcCK}gl!95n?s4DlW5b{AxTfg;YfH%yV9kp&m3yQV%mQ|g_Y`*x z*Y3Hgc2}@NM$Hc zOE?0od8Mw{w;K0IDVPQ99#Y{7)I^Q|YhJ03_6^8AQVM1PyN6Wj0=1nZz?xU;ynQKh zkCcL0!0sU{PJuOwBfy$h)+_r`Bd}6x5TFVh&%`5A!-E6r>O2I7P;PKps z0(%CI0Bc^^KiG{~@JJ|_1?(QOlPR$G;Rvwim3@%itGP!?!7O0+kR4fpJsd}XHLvXF z>|V`1QVM1PyNB#73+zQX0<3vuUuw5Y?vYY33)nqmhdjWZnj^rPSN7L-yW}1z1+#$N zLw5QDoDDbvta;^}V7E)|ky0=V*gbG6(i00ufHg0leeB$oW#PUSk?xaZ+04lk6aG}D z2?}W+yM57!yocTPfE8D$8WC2=6WVpAT|N8XxMevwha3R$V`=BMkSV!Q{lxJ8sBzu>_;B>gk*;gCuHF2BfA>7SvHOm=VZ z+E2wSZV}BSzu>`p5#IYKW$CA41@F#OEx*WnFl%v(j1;`;^DaY^F8x%j;C+y) zW^s$S0194Tc|BLw(oe+--f^@&I2W_HMcN8p$#~`DOl<{6Six(1TEQ%Ck+y=@3|^zy TyRF~|D|i)6E11PCl4bt`R@fV) literal 0 HcmV?d00001 diff --git a/src/wroboarm_22/meshes/forarm_link.STL b/src/wroboarm_22/meshes/forarm_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..fb511b3442ca5d1468b35914cc8edda55dd1078d GIT binary patch literal 19984 zcmb_kX^dt?6)lM)V-z$mnDB{g;yKl1Vpz~86=N|decJ~_&Z$5GF)w7CF ztK%Pj!0HX(Jue~dJK|OC3C}si^0<6PHAHu#YIdK|UjDG(&WD9h%+Lz8q71|r-gMaN zKYnv&;uu1x73DaBhyHFyd&%N98_VNnR6`jY#~yaX>i*;A62}littdy1t$*LyzU7|p zuZHmBk!mP|W8=0bto~=qA&FxMp;nY5$A7LoyS?M@-&qS``&U&%863C#`=HgmFT8aO zIfNmET2YQ1*FO3c?U#3MYD2hvNi~$=_l=Lad-9ptPmLqunAe@$?znVgd+NV7%&A5Z z=!Ki#@y!&~ZBKe>`-nFj-X3(%eY1*CEA&`<@TCc{>5TdItj$lgs199L4bk1GwqEqa zcFU7rVu+V7(+crY24e2a#mPy#o}M^{5Nbs^Wxn+%Yp*`@ZN_o_vT7)Ug4dh z?45-i!Z8B~wW1t3KKrhRu08l$A2N=smQ_PJ`n`E{#5qfeV+av~TJ3Q}=9rHh?Q#gJ zDG1{mmpqK47ok>^bFJoMt$v6++!r*I(|C-_cv!7^5mw!7MLBZV2sS?E3)f*h+zJ}X z;IQ!@qjzK7h7f8+IdWJhHr|N~yU|zKmv!Zs{mSDJVcp(Hw>ttFMOf$0mGk3Ck9X6h!+?Yov#Lm3=4&(D?hV+f&E zlp}|&IE}ASVe?2el)+)^)g0DZ$RP|N)QWQCu$8m%l{4Hjq6WS&np|IbJR)p$Ze(?K zc4!oV9xLnlsXyD^Y&m61^ZH9Zh8;jgsFm#o#${h})A`$))pL$*LO5!s&k7ON2;0dt zzLN>tu`kKKC<9@;nsK#TYPegX74lFk+eMABhl`opj;x70vUkq(S%ITQ*gkPS?h`M; z9xnT$4210&XR&{T?EA!Cgj!LKQH6yKGa+1OGw4F2V*r_Bpf$AJuplgwyi3*H>lFu# zCtH7Tb&6^Tp;o$QtT|Mpi1Eixo4o&-Ydsu_P%9k&(&8Tm5Y#B*%YQj#^5^qzz8{2I z9r@T}CMTTx%O1qOBYrUZw*!9T8jEC1KCE4ntM2GQs74XPc}yX4w#L0e?z+0FQNIr? z53?FGTaB>hP>mw2ZsWn~=K81=Je6}$`%0~B1jp=*VT5WFVLdkP?_EWxm7b4v=BiNy z&%t=sY94m3lL87QBjjPkdq^r-vdHeM0*g`%2ZUT zR@4ZGBUq6xhaEZEtS#m^9*in^@*JzaB6|ms^5_{s`jV(7kW)o^IH;LM*zaz=ejdh$ zQL7^CA}n4XIdCWfl|{tsBWM-Tu?xEhoCoGBcRQ=gbEO}_?yDnQkAnz{W)?mX70~_N zPL0M6IpyRY>M>UYts+{^58I91xoESG@AQEsjbf(yrMMB;Ew55l8Gt*WRz?!@6cOUUNQ zkK2gao#Q?PG{%t;lrhiWKLS>awwD7rU{%f>x_D8kQeb5vU>%SPjs}lzE&x zfGz?WMKHp2#S;1vYDJDbf}N<+2zusRub5w*x%whU8bQyzmm_NwVR9a)4u>MLm2vy= z-dbgi`dxQCGD5A$!Rj8k{Kt>%TD#{(@ALBM#urVs4umO+~s<(Mwq2{f+8UEI5;*hCJygiYSc=&y-RCEig#8K zka-*&7vLO3ty=Y^RxyuqZBQf9csK|BM!tgBzm&$@ITS$|9Eiq{o=1(KM*TwOar|Gc z27Ye|v!z+-eI~!7*E@@5O&=o7iL!FS3Oj)4>*moXue#LlKq0DfTz5;grXY4c;t#XS z7hlf%S2aiWrB?8SH7eyXg`h?e`=0Xp@gaX&?m?*LiFbTz{Jzt!$$8L|6?cF`5g$MQ zqVd^>+}OjR@v0S$;|_Fx9@HoTd}rMFi~B*S74o~~*}v^Um}WwaAtv-|QF+JoMwK;+ zum~rEnP-Grfup~=Y6T7%j{$`GQUvnrA9F>h6^;+~Zq_KmdTeE|cQc~4DxF{VmHEvh zh2GtE-d#Rw7S#jj5=Gz%#4FFayJyU6U&RWUv)}5dYE3~PCq_7oP%G4p{bq=Ir|-4G zECW7{0iuP>9a(i%lr?>b)Vp#o1HPCc<;16R$rOS;I*s6cc;r#8=wYP@TJd={cB!mV)Jxbt4L&Le4Xhjx{_rNrl>C>+-Hs+mUEnE3v2eFUw$rNFJ%W#*xQd5f-8E5d~3dl@YW;RO~lF zBo1m)=I$4A;`86@%oRbal=(on>zJpy`8khh7-h{t4g4ws72$clF2bz_qDq-tR32~Q z$Oyg5nK*h7v;vyvzgIWjZ@2xTW#%1?wTLoePjcz@hzJFNjES%)ePuw^Aqz)Bn5Flp z`h{#`9tejcs@bRFFw2}r;=?r+>NH)e zQeD@;N)d@W_U?3bGlEu$qdbYLqC&@U1Qo#!?N(P2v;qf4uy;WW%lVoD2GPKX9~M?v#AII26T z%)=41lJRhk8j-47Az~h754Tb82y;AARAnteG|a0;P@{e!^SJ%LaIfAe^0N}Y)GFn% zg1ov2%fmH_fXw6Is_tJQkBp$4#$&~P_gd8mV|I=Dh0NpNsP4mIJTiiE%%kk#YJ~Y) zfgkl7`3j=CpNPK72+ER&>uN-b#3J*4&1fD6e|2A!c{qYrG9K=sMx=^Xh{#v=i3jQ( zVU9PoFp$N6oXzN;`OBgS|BtY!u zDIc#9s!@b~Lotj{D~+~;$W>H?ehV^;sIAHuDqRG-8j)ZJfXwYI@bB)rBd8ihVEl=8 zj(p#T@1f%TZ@%{mnQzc`-wS89(6G9Y+0$KwY7}98EZ=`vm*l7vp;r9rqRYW|Xra-Z z_#S#MB7bYCh~doDms;^%?kxITxXb)4$Hsx^g=o!{k9gj#98)f`&g5cc|e^YE+VR3mClLGZozZd8g; zE9)5lrlE^ajUt9eP!VdyZx6a0I#OH>qE)GMow<&hBJ6zhRiewG2(_}Y@s**AP>mvN z1bsE=B5cg_7d%|sVWm~C^H7Z<_-=idLlJ6a$NgKGE<$G+euD)o?a(?Z)hL2rS9LiQ zp;ny1y9m`NLTgw@r3kgMD)@IgT@KYKLTl(8c@M|g8EcX4eeye^M0eLH)hGfo_g%vr zism?VIaH$v)@^-+T5&WZ;+~scJimJAjy8lFtODiv8J>CJ>D%+Zc-b7~c!%Zo=N{ZV zwW3@&_F9EQc>9uSC^HW{A1)ky2t-9I z%8_H6)#_HOu)RSyQVnJJMcwdxFmenb)QWQC_=Wj@w(;2gh8fjRhF^?7o?1qZA%t2{ zjvT2IjpJ)Gs-YbHe%qhEMtKMkf?Ad5sv#I+&AQzaYvb?l9YhWr^In8nQH~te z?X7p>=Hq8nLm7UpkNxe0$T5UaE6Rn#&p~_RNi~$22i{aD9DN8xMJvjY!)Auo=aO*k zl4>Z!FX~q2t09D1QH~ro!?ivqhPPtA%I`){1_#EUGpf&ty$H3U963_&wswAC-p%hu zP>%K4?QaW3c?e;DokXn4obTsH?`oh3>lAAmKc>@J7WwutD#n9 zjv7KB68cgEs!;fpLlL!AIp^yOhlV(&y8Gd;c9yx{(YDGD6{CfPx`7QfDU>r*` zs-YbH>R0hY2<%bHxAW!dJ_JLoMUES7C!?BzFh2Pzeh8sflyj}xSgQ&0$airlBM+;a z{5=4xRWCxVDCaS6eayFCfbno&&`<`4jlcYr0vq#Qgj!LK9M*}g_wK@`8P!k*hxM`i zwS^&sT2YQ1c1E>+4u*wA)lde9osaTYBZd%aMLBZV%+UH=5}t+gJm0OM3=Uq`LiX9G L7ok>^BgcOM>&LBF literal 0 HcmV?d00001 diff --git a/src/wroboarm_22/meshes/manipulator_link.STL b/src/wroboarm_22/meshes/manipulator_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..6bbdfa566555fda5fe5874ca1470c094b41bbe78 GIT binary patch literal 54284 zcmb__3A7c(wRRa3ClpZ#D(Dpv4wEv-Rngn$G&oNXj1x{zoDlOH6-2;@fU^-biN5Eo zQG!Qq2o4d^aQmD_9vBi)uZjkYW8(B2;)MJfMc&?3dv||zx*Pr1`rlfvw{!OX>f2Sj zYFBmN-i0y$fB!G|A-6X(G>b>}GiG_Isq6c}xKX!67hmv6ndM#EpH5`{{okXOl=Bg+ zX+$u3$8%;YqMbRGCLD-hD@MnYZfeWtP>pgZV&Uya2fzXSK!jQ;2lp@A4@PLu5y5hT zh-s`^X{@T$Rs@Jht@QU@o_xY_WDu-jD?})t^(r>)%&|1#K!o%>;&=cDAXK9qitys$ zd4&kIVl@Ar-*eTl6(U$x#L}uUO(mXR2fei*X$#v`bS}cq2G)?3C+oQJSfw0nrR*%L zhs2%88H5)DjV61mau$IcDr5hA1ZvR3Vy_T^R{VRuSEy+~B;%I#O50+D%H&}(B0d5& z4TvB=9)0HCADnRNyLNS6zv}j&d(Zc+eC?4p1XB-tCt0;=1oxi*NOa!hWmYrn!g>;u1O zdGUeUY+IfCx7w(2W*E_B=V0NuD2D?zh@epq^5cR1z&PU=`d*<15frCD<5Y}5t75NE zg9w@>fzBA80}*J2BjO`ag9w_H1ssS#s}6tLHMnXcH;;V|jNr;M#<#SOzdRF7AAwOr z#JF{LEqdAaFh7 z3Z^)HkIaCG;&qN~p;hsULJcBl6-1i({CFS&t#CE_2%HT#OL+Did*?rM#tJovFsHQy ztAcgkfj}#sZ{7QET+YVa(?Gk|{BEOQ@WY+H1A$gNE1O1k%t4?A5eIC!Ps=Hfcl!Hjm= z%9=5<2-F|~`FyWXgNRe!{c&yX3;S8VVgyd1eqI}K_fA%$Cjhp3`mZ|%H~oJ590Y0*p=XpV2O`jlybAKYLMz%o>XQ5qj>*dW8tIqA?6~zWMPuV8H{yriV_m>tOzN~_qF8E$`d2C+r;p*`Bsb|bYg|G(np{M5!8<;zaNM|tKS^Y7nuex+EI@>a6cVfYq~a zeHNYn$?_ZowiR#kY;^Te*IKJ$1Zogb*?)C3r}^?64n%lXcXzDaG3=@wgaHjA`uyPa zsK>$Qe+L4su6?0fZJUEXY|G9>)F5K~r>{l-JodO84n&|;@g9X5M7-Q(b@bj*BXT$p zfmV1j@Mj|CTFQ_1DV}>m8t!M=Y`I*7fK)aMN%K6y`5okqgKFH4us6j;W+(iUh70+GN zAOhElA9F;Y6|Ng&-alwc_~GSORE&AN)Y`ed#h80~)Bg`jLl?BPb{{qOdu7=czG69;?5o>7nHpax`WcQ|`$JuF7}Rz?wyqwY9PpubU| zCT*Aa+R}cISGfqZ0+~2yoMJat3#zCAnMb#c{A?MJ zm{*8ED~Vu^SExb6^ZU29oZGdM!+{9jD&4952;#2qBRbKJHadp`5yqreWX)?ZzaOAU z5yaP4*pCE(R@8nQ_c3>L6AdD0+@ivGBnY&^Y-h|KfL%|AY{ml~AKH`;O5pMxd3-j$_p$Cx`3TuczJM))Hz|W)7T{#uOpY zN@d5f?$+ty)}3FXclAq4s8N|YaNQVFgg`5m9mj;J7laq>eYf2?QKK?*;F>q42!U2A zJC3MQ2}?%|pc6nuyR*wUtWlXcFn5t%0RpX5b{u~R+bd14olP89l~AKHb6^fOrU-#n zDmxCdvn5&eH!pPWUD5NM^c(agSW!id%FID)B{^%^^^uD}E0rAwt=ibF^A%OpsLUL+<{`rsA<#-?$3gijcKK>{ z6*Vd|2jwov)I|uiQrU4(7OlH1dSDebDl-S}B{uKoB7Cbf9~Wn8L=a!W&Z$=H@8nHC zYWwxiKG8gHd+z#y=Mwe35wUNtaZx?|yG8qZEh5lLM>@+vbIG1@SVMD{cjGpe55Mx= z;E6j1MOlPq1J)2nK%Q7DT3zu(|B80=DeGWwUR%vW}1dgJ}c75Qe`v}w^g636}zoHOpyCbu|Xoa!yIYeXUk07jEoUc%W2=UOOeGWvR6|NN@ff_`} zNat`M0HUcpsMPUL$<>m$)1LgbtbmxHiY6sKg=eGb%MyW(w5&k=!E z7#m}*8FGJlS<`Oev10;yURzn6QQy5hq0`Pa9G^RmECrkWv9JBzm|y?v!is6?X%VPF z1dcSF*adMMaUcS%#!kC8c;at62**R;EUg^+aL14ls6hnA-u&)*4VZ}|h^ z_~V~93d7Iet}q8`5aEy2!-N=~gFvgU>pGT?IdfUSV`$9Kt9A&NmF~41s6hnA#+c>; z1X?vs>RBEzVX|=CF{W=g@vGjJ12u@i8Dk71F33Tk)zDw;RzB+5`&(Q;&OB}R&@^w= zkH-o%h!EMX6=N)7a3R84NlR^xL;6t%4y(a-rB9K|KGKg20Eg9uqG%Hh^W z27y+x(uCuBTD9wU>S+S%;kb+t8#VQeIhP;;tz`WR$5hHbPd4pl5vW0gF+WO9 zvc_EP@>K@mTMg<%c}zH_47tBX8P4{@7}TWg5+6VBNIxhiT7=7qDFUs?+lX?aa8ORH zQ%+Q`5)C3~-1wh;pKEP|WL;6Ez-*1`!m0IQvje%tfFT&8vuV zqHs`7jCJm!1`#xm;S5J}H>R9u=Pn}9idI35a$>p$W6Fs-+Y=2UXg$JNlyYJ&0Pv;qU0;eaIv{!^H#}Mi3SlO+r8c9 z#MsO%L|7}~ki0AXr~rr6V7t<%Ajyf`j|>8>WJH1_C&rW$Edn)&kk|+Z<;0kBVg`X$ z5_93;S)zL_Y7ilFOgLm-Sq?;?mCUztk`wEc6GO>~Jgcn+5wcd4!>x}D0%)J=+;W zInkc_c&zZ$hX~?>=jd*;ZmF;7-8+8bnC2Ms(wQ@EjK==DL+!_h4XWw4kt`cCfbD_JjDyKYvr-ct~@M8br_+N%qT3huHMP zE#uO|U)6syW%n30i12gby{~tQKOXY;`tQazdkD0;$JX+tp zZ*zIHG=80EUgY9Y!g+pw)t~oyZ z{5>k~O&JuU1`#rDl6U)z8W8u{VM1l~dCeXItt3u?<5>OIp7HtrnqB$Lw?j38R)dID zQg7lApHAu(Uw_XZDpLmzj!}aMnPcMBFVEgCK4$vz%Hb26Jp@|GoEEQ!Y`R5!_hlbd zp8ss9&UULo1by9VD`K2iuT0t^e(tAVRDSl=;21TCko72e_ng|AdbhrtgaaovdkD0W z^)Fs^Yk#hO)IYj}m%lSqGlSJ2LMs;}f_pw!KYiAA;g^3K5~BtYxUU$~?X`LJ1#5N= zXI*}GJMXd%ATDjm5Uw(RN{k9_qg!}$wj~F$Gz@5mL z5yOVmo4O7TPnz89A<*itZ+4C1&36#5p83;*wX@%C4o9pQ8lwgg#yp?Ae=z1fLYza0 zas$G*T43vRTtA-s(}QJ2(AQ3&N!ukpc$e`OA;ybWh(IgyHc0#Nr}CwhwWs$Fhm%({ zKFK=^ia|j8pzST9joD`2<&|mM4G0H6v`35@M9`?)x+2#P8r_PHE+Wv%_iE5tn}y>Z z{Is$kc|~(6dCNhwJ$V~JE2>QUhs@nEv$qWI{`S+#N8<+bib^#4jW~OPEg3`WqoV5r zHHe_~5zx*l9D8l?Y~>eU{WuI8yh1A+Lu2-uxUw>Tao6y|8-{qJiwLbya$`m7qoV5r z+d?ZGX=6V7cyRdign<=~Amym!O$uevN4#glBu6Pdhc*$$DWlAn+)XrBhl!0 z=3rL^k@?dPHH=hzln;as5eF0H|zJ@{#zXSNycLh>!>h$6GY7x{YnM5kv%9VKnIt z$%5YD_4m~4rytZ`*N5!~BJ_KDH&%2`4E3DowI$!svlY$-Vwc{h%&dQSE zYL4T9Z_laRcE&#O$RkR;e{e>&8m)q0D{P5&&RMrq;E9DZwQUy>k{N_!PnWM^$n6$^ zRv1lVF73CX(tS*?`1ooc&AS9(DO${pFglIw8FVSe+4-H?L z7`<(~ZLiwz&e$`4=n#4~fVXNsV%I)YMJuB#yK~I@IU!*hvaYQn*7A)4kGKSvA4X_KRRi#a#k zbl3lgP=g3PKQgy5$NuEAdbcZ1C5{`bh(IfqT|bVP)H#0mfFlV}t)fO{ZkJ{vKM4m8 z@#KWA@w{z*65c-Vw-IU(p=WL9RnwKb#oc!;6UReUM4*+*j^o6w_ls{{TOveMMUBeb zE@dKq5*DwP3_dU(wac#IjdPxgP=g44E^%J{andnyr=zwaj^$NEpq0wbs{tRM5%0ar z+w`^J$|`D9=5{H=@sqH4RlVk{c&~R?SB8E4S%exy=rf%2>a}ST;txM=C64wgBG5`@ z=T(OCD=rZ|5ipRg`lS|bri$E)tomUISOpY%)Xci$BR8gZcw=1JAJLfA0ToAAQ@!j>N+e@AX z5&9m*d9~gA{}WI7=+DG)r$*4VmDa)56ydzO;k}dNtq)yKi2tm58bHVl6|V+3n)%8d^;=h8 zKnODjH7awvxbIT&*eTQNH_o3DZ+!fIo(2)Ri#o5K-MFRx=Z7yQj={aLEwobEd3EKj zGiwh_pFxNPy-}mGYd76@$@?Co`i**YMts<%`_pccY7jxP*1?huag-fLN2*=&5Q&u{ zoLA%KuC2^DW-NJibBTJZRk1aShgPp9vO^sCyG_Di$(Z=o<#Rj@#fG1@ZJes(LU%ud;WpVf5*Yeg=_gVuTI*7@=ht-C~%hj1JhR|bU- z-9>LYsh$%NDzg>F#+Vm=wRgDRq#^Oa@BEjiK?G%nRG=dsOK5sFfZ<(QM`Q$V8gpxK`M!S9aG& zZ;he-Jaqf{(h_RKD-YqkqWwIS{oF$+nys);be_ECtWeKBbl%as1=RO|5$LTkboL3| z*=J4(HPTxT;k=@=Pw37*4|xbhgIBgs#?U!2)N>-84fQq~T~U?;y`@|->6p-+6R#+t zMrMYGa9+_lF?8p|J3WM=*$Vq)44tV%JyX*;T5tZ*HwBghy`>vcuiPzkXX;@k)X0kR z5Y8()Q-|(MJ;p;Qnys);#?bReMV~+DnM3b9;$1X(%Zc9F=Z}he{#e`_HF*f<6+M51 z?)k%%5TP<#VV{iY`pVkcfS%JsGqYD!_Ca}GS_JQU@yUx2Ws5MsrrIUcqzK8u(h@!0 zRrKi&5om=YVxJ%@?g{dqIjG4)xSrGVV8uNTzR?R2Dzg=ioBa}y_Shz>J?H9|v>%Fa z`%)*eSv<$dbTUp56lc9FtVW}&_aaf|EJC0L5yYo=BoP`p5aC;;-uej2AIa&SR;}JB z_C38q1g$Q;|7-(Zp%spZ?-lK2_Aj}16yX$({qqr?1`w3B_0F0Nc!gFtB0dM54fHOp zRZrCfd@;?`*(3^_HN=7$_!`Gw;tr5t2g$T5g zxsbzw8boNVLzV*(Xod6GpSw7hv=WWm6>llJ*?E=DI7DbQ*MsJ7qZ#p_NZ;MgJwM*^eM< z5J7oGD{>pWLIhgjn)ho)jTvj#1br78;EGb2f47zQN$o3YROWu*nlDD6mCB9-D{@hz zGIRL-C_%8mnTkx`>EbKto7wa9A4ZK0LQjsq*CQKK?*U^H#5eo}?4R@@d^sq8qg z(i=4@GY8H}zcyU0xGl6&*>PaSI%-sA4qRQfRzIn6S1WD{tyFd#4VCv=A=!v%k-J`a zw)@rnh)Ar6!^%lNDthMtYE+i7vQ@t<`*#kg6}LrwN~~0N9C#A~YEfeg@ zWq9qHRw_FVyjuh{Ds#J(yR^pLWw_)v61C#C&`M>;fj6F@MrG%FdVcUL z@72n+g;r@kF3!|=lA)Fg&R3Y*;XCc5PmUXRXY>jY^yRu%PVyRrr#nQT6^>yBp*IBt z@U=R%i#Y7imgX3;(eBG8I5YDBA-YknFyG!L={vl@*8R{(QuP6GnlmEKxUiaAh& z2pPAWejoy^FtUEnQG*DHW`55bXNk4K`1>5Va~ktaN$#0Q_V(;pVayRBb2q2wh(Ifh zzwedq$jnYwdhZ8&1!o3aAE-fufA=jmfS5lYC6>c5^t>`;Ttz#n&sY|s8)F6UJn(I=9L+Vm92(+Sa zBejB$IHWF>s{#2t%S3|+8fmUe6%ME$^blx8-@|hCDOaTs2Uj&ptv-Lhn`jV0vEjN@ z;gGsi>lGrCx>VtSdQSefHqju0Vy^e%(E5a zxGt5ybxt&h5LtEuix^ypuvXF%fBj7y(vLcDSPix-eTrQ6k$z+lXeIsQuZ<-hQme0H zWi^P9*a(NzrCJVb3#}wh!Xb637J(W>$Q%<6nO7METFDIMFHfl-ygsB>UuUA#AVSuP za-?B{t}(-i3Sn09^qUf_m^Z4Xhmz2t8RpYG6UaVViBl8 z1mzXIhsWhazP}`cKr5-)kbGrtBe`O8zQ084KQvz@?FzcZ$%&!V>T7L=)gVG*BOG#niRHky&`M%196U=xcgu~| zu~-cvWR3}k%qz=*ZK0LSw{nsb`TmlSdyX1J$XZd3^nM=?fmX5xg+uNyu^gyDgsget zko!wQ$%(vbZChw1>t8tJ{t}Bo4I=O@MRI>h2H{(!XFHn{?WvE)3Qv8AAU?joMEU`h zFCpe7_y)49f3K%+O(>U8t$syn^*sbyVdkTp_)1)nTKy2)MLzv&xZ|K&{fgA;hp0gW zW+hvzueCBB0W5ORAEE{k`uBS071ip8QmY@L1`&Qvgc>XlfmZPCAbCZ# z`k~b7hp0h>es|!!qFVh>YV||ZAOdrxt<~2WEDwQJ@a-UZMYZ~&)ar+*L4LJhyz8xg5s8&CeT78}E)+W8R7 z1nw)gR$pt8Jp@|8cYWj))#_KIR$sFye}`B`gnpyryrNqDiqz_ds6hnoM7CC6tI0hC zTEVx9E1_tT-7Ksr&|4* z)arW(v?6c0Cen_ftun1ktv>H+$vX>*f!5lU5p8Sr>r$&9q6QKAb)$<1+=SpE(8~9U zYV~8O)emVdCGRq5wkPi#Xhms-8;up!>c>*6&nqg?&|1+OFUr`Gt<~4NEIbZG(E8BY zKH{KS{kqiZdkD0`F|@V%b*a_&Mi&vZZtT~lZmi%g3lD)-IMTLOKbBhkkaAS=zJ#)9 z@?L{7wN|)MKd4qemRfzz)QN_2u>Hc6J;9c2t-juo;&C8?@~+nQ5eL=k$5N~BA0Xksnrirg9ysU_A6F5R&Ym(hd?Ww3$|82mRkK##;1)>+tS;JW#Nk)2i5AwQmfDV z2lvowXm7V)uCf)jWNY>H?j4T<5i;s+j)Q9TW2)6}rJ8CFfmRqzW2ja?mRfy9=#I?M zwC&P9uebG3JmBsf4}n%V7i_J5EVcSNx^{eI71emGWFE^{QLTO~wfek&@XE0oIy36c zPi2X$ZOPW^>zz&>2O=bb!a=qAvDE5&2(-dz+FE_Od&k!5>rAxlh@kVM{r5RtJm5|z z4}n%V7m_T>wfeeh?f6K3w6bKln&Y5a{kqiZ^KPJ3>io=Mze8p#$t4zTYxVVZF^>Zg zk{N`9YV|o^ajm|GKr4)%4y?8bs)K&(15V)h|n}zK8Iw`og<2s?{$`tv>CXsm4ci z(^`F6QL;O8t-gn#NF`Pne_N|xlUjY<2Q{_{LciH|{eW8vJOo?y3YNd8rBZdb-l zP5@BN(W0j%`a-$E%AG#=cU!BUsmPzR}A>5i;5k!5{1 z;Jm^*Nb$-d&`M?JRbCw=Yt)L{l~I?S6KY014I=bCit`FQLTO`wfde0 z5&C}4d4)B+*cMu;?7X5{{m|9w-=0+cvPP}AUCDg1pHr=VMQZgu4I*?$bzWhmH@1aV zDm$;JR=?tE^~WWZ-mFn8ZWs4mTdQA@T76H02;D`US6JzdZK0LQ&MW-xRrfw_OJ%lF zgyZQ@@ zt$yrk^@k;QaxjOY*$QK0YxQHP)%P@rpw*l_$=F)`_y{mdpevQop-d3R(%f`f!^9${p6k^)JShVg!3xzo+9>2(cqQsldaX)JDmbL8*25kt|-fa z-r8FIbm@MUBeb?wYR`21ic0S~!XjXr;2_n0Cv8`d72g zBMx&PYE-ry)g{63S7!-FAp)${i0nApZ~LZvTZic}6%LnBqcU^En>-s_eblwWQG`G% zl^w^4gSQPY>GMzh>G+!Suv^z`tw4?m!ouPC8LW#;Jf zgV%!|2cIt-MF_M~*>UW!enxozJM(BYwUSx1`KjT5 z&Htpr|8g&CRA!EsyQ~i0J8Fb*6d}+`Wyi7PtF6P9`A3EHR=hWARA!D-mOT?3J#wgU z6d}+`WydjV|98tToJ6;gnCtFCjmpe%m-anlA1M-c+8RCXMPzi~-;@Vy(;SS>A~ zMrG#c+`lzgc>B?TkC<@STZ2)9h^eD~8{Bll!hq4nFvmfbV{r))Xr;2_`1qE%a^<+6 z(`>)2gc_B(-C-Bb3tk%0CLBcwv{Kn|?0DM2;qAZaK^%)qs8N|Y_MZPpaNgu)!cl}k zE0rC`?I%wQdw)EDR@BN8YE)*9iElg_9CGUO!cl}kE0rC`%EKpw^L{jsA~?5%8kL!2 z?Tm+mE^V&~M-c+8RCXLazUdntKXN$D#9<}WsLULF=H4HiaO%6lQG`G%l^w@FdLCC9 zeW?3az)_e1XB-tM>vWQ zXr;2_`0rPT#f!TRA&$jW)TqoHokv_5Y`^jq;V43&mCBA|X?;q3!k05C=IvF~sLUMQ z)?6Aq)w^9diV$d}vg4pLLo0oSMg4gHKGHbnX@*&`M>;L9@N$*2nN(s8N|YXstx)8Z1JfmCBBTR&Cv_ z^MiN|+O}AuGIP+HkJ4OHgg`5m9S3Ef*yXDQRn(}=9F)7FG$$4z&`M>;L0L3*d3RM6 zH7YX)<=`mI?L`Q*QrU6PZV0=T}jqGIP+L z7^Qn{5dy7Lb{w=j*WG^J=|0q`%pA10N9mcN2!U2AI}SQ!#O{>QUPX<{%t5D*$UDiz zeD)dRPBMuG5p=4F((@H_xYH8lU1$rfRCZp`iL7SNa5H;WQKK?@Md!IFJtr0+&`M>; zK_|=DokeF?QKK?*(77~9&$~qkv{Kn|&;K~G+>dsgdI zMUBeLLCKKr59U2R%X7>@%mit~Y8_W)6D(jMC@YA_Q8g>^SI2xZ<9jr}J~I zZHqN3TaK?6M(Oi;Ap)%E$=f0v2R+e;?j6A6CDf?Q9P~aQO5ZaSA<#-?$3gEhLia9X zWeGJZGl#wjVVS)R3HGb`7rDsNIuYJOokWcK~X|8kHH%EiD`u zNoL4HpcTrFBby@^p?J{EafnT(|_`q$}Lt}~%Xr;2_c;K9E!Y&W; z8_0V~s8N|YR-Q4wrG5P6!cl}kE0rBb2X~upY#!=WLXFDIF>c*mi=I3A65%L9pq0vw z<1aTnTHohnek1(G9Mq`H9E*A_Y8!d;b;41EKr59U$C-DB@z0jb{zjrG2i#(^N3@n zjR#w?MyW2((h!arCCS+n?`_rnj}IQJFb-9yg8bC>%uyv{Kn|JVI-5 zn+JbN9Glto!ELcdt(b$?WB2|W3r7(GtyFd#70M;UKKY0uc&lCK+!kxpia9Xz8B>Hn zE0rBbUKVAIS}_OBF`H2x!k&3d6WusHGgM91lbO|Ywih9I4yYCLajxW+?EXRbUpo$t znr0au-88GEXdi*?B7(;)&AaFoBG9VXtHyPg=8x~=6|X2oU?wW=2U_8X_z0ZgxT3Is zJ_0p}z%^ftKr0*(AAxfT=Pq*l2-F|~S649tt#CvvBDpD@p3m&x0eM!_^Pv5E9m`E! z-w$?ee>zHLdxFS&&mf58jfKxaZ6z9g&%g+MACTqHCsWqw6QpwFd&L~8L4-aVW;qan zR>eI>4I=c}Fqb14MYPfoTb0`9>a4z^9G?(n(8o4d}J06XQ{X1ut12u@Cegyerh2u`KvELVP zyBH_mE7Tx@;%~nk$a;kcv|_JxM0^g^AcAJ4{c<47;UT~Z*T2t!*?{6<|H{toVoN>( zHHe`2+rOW0*ekT+mh$6)8br`Mwtqj*=g_v$3g-g#1OB>19LdU2`Qi&cX?Rw5iqZE5E5rZSlMxmKJUcPbR5)g z*6NL8=7=ZV)b{UDOUj@0A5do59RIQPYTT$>BL1Cj_-sI^1`#at?~JB@oNEj{*-h); zI_CXt;s@uRUt>*t-c}WsnS*8Gj`;Uoo_wMKftp4Hqa6Yx$OzWxSg94F{RpD(d84q| zc|GXNiX4hy4I)%#ZXdDR#L3pH!`C-e{_~U@tUTkTohw^Szst%mUcE~L$7e_O3)uIb zL66D@oLM4*-ae#UfrLWcE$8blP2E+WwCptly-eq`f;vq9yjPw#AJqRt+_AJlH7 z8blDc{XN?cM4%OUm9#`0-;R{OhJSb;#^d~v*I3{COxoFtjgP<>AfoNi6D_*_@1}+b zA_A>&zWF`J`Ln~p`|iu55KSBKJ-tE%f78V1*AE}eUO~^%E3~5D+Zd5M+d-4IOMHve zL%$!`7FtpNY)jeoff`y-)=JhOaARgb4n)xC+L-fdF7^toUjLtw7V&k@of{CS!MuXp z#Ts5l_ z8O|DB2fRL5<`u;&jpZcoCJ1I{4Rff>zO%RdJ7)>v_;#cqKJ1MeMDQ3gBAL4h2S<=K z99xb#%RDoR5vV}~&oM^xtn_=12(;ok&4^^SCth*pWR0ExSjH1Tl0`EJ)^Mg)gv!az zxj_V5VPyS&=;$_{6ZCh92b_IS!#S9};&Xz^*?#09*h-_BI}>fYtig6Uw=-XsgR>}W zc-$!-R@VKTf6vYmilDV(4I(K1Hcm+{Nk$iA%NdT_l}v48p39MF5JCSIcZtzSrWTG5 zd*i%9EA-Znpsz{WCBA*t!(s$l;TZY|&T8BipRaIr@mY(1=d+fNKn)`N{!wmEMi&ui z#o3NIvbh8`h|qbJtj{8h zkX+J$P)#0!(Z=+8bcfh%|M}dWj~@|ooSB2ynj*Me{+(NLYp@Za8bq+H+%$LjZ}ajF znIoti$j85HOId_!8W6r$q3Qc|U+xD-kU2OzBOgZA_S{1-M*=Gh`^IYBZASA+hLZd z6|V!8nS*8KkoD1kP)#1f&E4JYy!v`>_s5TjdEGHPXBkCsySN^WX+$tbBf|G;%e6a& zX0w0yy?N519tXCICntIkEy!5njFAJ|_4mOd1X{7o-nvuYZm~k?z_emA<&9t z=HL?t-MQR!No?8=Z=Ez`hdOEyf#-)}gm3i*JV6piXf`>#A9LVSBmXPyJkor6A-?1s zWI1vXXvK2c59)bnb{^J`#|kxwpmDQVG#TAo1X{72#)Bdlnnhjuam-PJ2#UYua3}pD z1X{7o91cM(B*yKjAJ*$m&Z&-y!O#Hc|8o(3r=79h|HpNi@3wmGeJ)9L!T9j=`a zqXrRp8Z1Vj70cWYcNV>TMgQ3J=+Ju7!<85{h@c#7MSc7xD#axtf@ tZ#A-KBv$-8N7n68jR@6XyU53KIL$`S|frviV)7L{{>(J7XAPL literal 0 HcmV?d00001 diff --git a/src/wroboarm_22/meshes/shoulderBase_link.STL b/src/wroboarm_22/meshes/shoulderBase_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..bad98d04de53b49ce43712a08b7c6213f7d6b192 GIT binary patch literal 53584 zcmb`QdAJ=_neK~7`K2--A|9LC5PnR9A`K+qr{!UF*WxfsGEgUpB|hY%2<5eJkZ z0>U|f2)Q5vwvcl+Y9pPijX*n~v;znNQ9zWz+o0a{uG+uaZ`ImWKl)^}U)ue&eB#Fw zi&J97`}W9}9Pn&ZUH^b@U_ii(u65*H8SHD%LZeHV|o!l0k66-zE z$)7oP%VEX!d>`6=WM}P3zJt9^eeoKDScX!z-EtS{&pZ}8Qi;kI6 zX@!*VQaSPP7pCXuJ-PO9JAq22I{Uy4^Y^dzt^uw4>}}J^l05a$)@9#>*rN+pzH~qi zJc{dtB2D?k@EZ`QL~D!<&Os|mwf!oqsojCkAYd7?V6R1SfZfOoY zQaSPTto396w-cyDH}{8B-<RFXAZ7U~GiT=?a_sCyA|I`5l9;uwr3?4i`oIoY|*4ICjU-!jB2IRmal@qKnX+4}k zCHmv%ot$sC&#VD`jz=mdnpbKkP>Fu^Tc_ryt##Fa96Dxjl+h~6$ba^;uMQ6$?RXUT zvg>9TQW6R+P#KjzsMQs%5Xa6CrlPQO#?PN;zcf ztVX5EMz?+ZtnQXyJZ$3I8(lcQ?S>oW3ol!`_|lsfR_r@Juxb94D>r)s0+k|RE5*p@ ztp|L(yYZYq#2k2(!|kn>Zkf;h$>BwRG!m$k16w(=Y?VdB-J_qI+I5d|ppyOXSMHe4 zT5^6-k46HOB4HOfZaH$r?q>gXK+J*1fAm;;Z;mA&bYj>zY4_O)J)CIXdm zU@OPS*wlG@=D&UNQ!xh~k1PA?=TFL4Kl!SdqnSXZ9N0xYF1zE2{Q1uw8FS$AxU$#y z)j9cBZoQ;ek0t_@a$pxZe(-^fJBKgbIp)CQab@3n=z@I3<4^15Xd+N42X>KTY=!+h ze?4v0m;;Z;mA%E$7v|^QuzN2@6M;%Ouw#yFi*J0g^Qq^)9_Jt)k1KoEB^TxEZ&_ST zWQ_zW<-k^sk?X&+a4f)7cmDOk1PA2|G6Nax$QmiswitDP$>s? zFULD~>zsAjP{%zUSN6pR&d*PO?tOQ5XRgsXZkK&y4m=)L_Qp@0nUBm~ zQsihPP$>s?QK|>lz9ZlMO((@1cs#D`v5y^>zi`2ey?QhesFVX+IYx%wJa+%^zdSfM z=D_1|WiMI&(EQ+=UynJO2~^5~UDRWn$s0`Uc;lyI4m=)L_6ke(&Zn-jv{#QN0+n)L z#~eeut^DxBZnqy3bKvo~vcG-JF8Q=S-_z@lCIXdmU>EiH%|q*T*V7;=KYH)(jwie;=D_1|Wv}t;&GKs(Ozq8&CIXdmU@OPSl$Uou zp*ySlW@wl?rykg`0?x&_MZYNMF2X-XJ-g&@wo%L?rx_i{r+s5zs$07N3!zb_1 zS@gY%{lDKnC;!_@o5Z#nX(Ui761H+=WB0vqSZB9IuT8i|IZ(+S+vxcG?&ZEy^hYCs zN|CT*Jw_kB^Nh~s?QI8ka*|xLu(rF#{cwE`*eCv?>&Bxwe z|OGo|MmyH98Cl&<-jg-9F@(R{`SAli#hOkT-g`@b?f{G`#sRh(L|t94(uYw z!>6A>|fn%N^WZG;M{L z1CPg*ec6Xs$Twf@f?kd$0+n)LE62!`|5)R+?&`06Db7JW9#{6I8%)aQJiUL+(M+IH z4(wQu$$xruUiZ~GD-XNJUO^P@wo2d@wl?T z{>$a_oBw`)v9>o7sFVXcw$Qf3vkN?7h z#d-T!6M;%Ou!|hs9j~4^VOKE5;nn1S;jg zE^7X}^A_ZPoN-1s>;?%EKb0mbyE)}Ki6Dx+cdTjY^I_~kfvekyW9{c59jwS+?B4NjR3_Tor@cGbQ z?(w*?)w5Gh{#=oxkwB$L*kz6ha^Uf}vX@?dNWUJ+F_1u|9N0yFgqnA0D?A=o_Sd#O zD7JJvfl4{B3qm73jH}p#cs#D`uO2w7I<5v1sFVY{k|T~?JRX;P{rS-=RYItQQc;gS z5oT1EIf%;R%2pd{1{bAjB2dvEB{3FOh6$C-JswxK#)4Lin4^(ErAXK%F>v)kr5vm= zMLk{(z4vUdRP4}eRkJ&orCsC*?bK22l^vB6vbhJ{PM}gGY!Ua3J{RWSW4-kXkH?kG zou+mIm2zN5VoEPZ9J_ctF8R03Eg6d5sT8&$P#XKD^kj;T|VKsirGOS}G^1s($^dIYQ~mnmZ9% zA?8pkREdc#Zi=<-<_}B{`Nm^O!V^0eF5N2r{?=n&nqDQ`I@HqHH|$$5lj1*IoBGVRkytV-Ce~m9JgZdbr04tJCym!c{gF zA~DkKaYKx7=12IQ=Y_`^G18`l=7eY`WYbnvf-__hI^)pug0gu+FwKFdPWahtH3w&X zA~-vfttYF_Kjy*MAI$_RXs#xz1bZ)1`F;sc)}{5pQzv})qng7BRHAu8FwKESDkpUP z$$m*ShZCqobJdsTpxve799}m4loELA1TEc8pb{OF4dV)rR8CxU<(u;pclhFfKL6OM zFAeYe?O(;+hzB zus_pIpc2jBH>3m}shseApXw-a0+nd~z9G$lM=B?L*SVU*2~?u_dy_N=9;uvQqK zfl4$_v8MzcshnVSO^KbK*eGX(`|kHQiGML5nk)U3z$29tiuCfMD-GyDCs2uI$0E&v zM=B>)`po2f^ym*3&mmXNOPoL@nqzyK1CLZrXjK|~G;jiyXpSXm4qxGvgX0IQYf9ju zR1ox2JAq2HRy*#oRA&?(shpsn(i~2p63y8_O5l;o35`njud1bT0+ncHbDD$Wwp2RS zYGlX`%4IkMa6-rV!DE+Lii=7%NA0v!cr>+usoRA%&mTlEcpbA$RaaSbG;U15x-=VYY=2ef3vv%`qpUlHGd5qQD z#VOkx2Q4mQ94KWGhEOOj@vDLOGre_gUQIM7Y+vUO9?gwI-sf@;#YAcdz=XUA9EDrD)jk4 z!d0@Bqa2Bm(AW(lE_yT%T&3Stk79m=(Ox0&IN|w%-{!gJ@VLI4P$k^sM3|3ZCQ&NS z;e@O7JL^iC1CJB3`8~`2dhm9!=Ih+Fb=UX5rBaJ(2SL9pcCp`HC-6AY%a@7Hve#$6 zJ?8lRY5P@j^x}>xuJ+qQq|su(y@_yDb%*|dx9k~5`;$vQ)pd^(Vcv`?hwgG@SrbuJ z_4l$O5$CybNTv2EV)f&Oe2e?_2ss84u9B@tWz9qEY(#D49w)RjsT@Urgtiiq)d^S0 zR*tdh*J}((j5|^kC@om;(Zj$L;0ojS?wBH6j%aSf_Da?N%dj$sdz=V6Y^uj-Wp^-^$_ZETdwJwca>(O^ z*>Ptg0KJ-fELVj&AM2FnaE}vKr|F8Xe?{v$;VNsx8lio|fU9+uDQo@Rkeg(5BE5s zo$a6-&{j@VRn0Z`<96kEUe24MOw>G#jM!@$)%1sEs{LLYiSBU%I~|FG2&uvti=!mW zNBuYcEBeDd9@ocPm9QMTYeihm^H{%P_3<3;al&$_U*f-_R8F`mtXDB#u_ij<9w%%} z*K#=Fs<3Lu9F@DsakRU~iO|DSLRm(`{Ls1;{}uIcqN?g2eX2w#yUH0?(a;Kt7gtUj zm9=`f$BEwB-aFzHS&C9QQB^h7!^`f3AK|L?aH6Ves<|Ioe9rrksG7q)PWX9gGvO*7 zXM?hL7Ot`%9G-^Wl7svUG#|hb$9FreeZeC~lv^#Ydsf^_+{q97T-M-)1ojt$*{#bK7b%Iui zZ8f@5_)Da(%{*7n6jjR3n|r*3S$5<`SB<~__MqQzs!n9tj04`^Sz&C$u6vx2&2DrtN}32% zii91BvCD6HJzsLuVKE0Dk1Lyf_;v!7a$pz4!Wp;b_pJWOm_t`4x?|!h<>2=!;hs8C z%@Mh9-XN7`C^?9$Nc>*pfN+m!C$OV-=(#m-%YX6tlR8qdzYC!wry}utm2gj;P>%BG zJoNCiS0IkH?i=U45DeRLX&^9L2FC z7(?=~|1DcPkh+U3+iPAW*r$ZnKBa7SCes`qmwTS=Imj-R6Wk?Db2x!YG;bhE2|Q9c z!9DMkU}u|BQFC^qQvy$&pbgszRHE5iP6<3xInmr7>=ldP&X4ToQG%yVaEB%>l@q8$ zGp17lk5o=DLfZ*cqJ5rM$1WbJoM7~%Ii#YWz1^By7Z3H&v#zpfku--Bs6_DGK}z89 zxUxB4X(v#L;E9Wrz~gacb9XT%e17ouED_B!3XfDyRNk=_&)Gw^xL!GdN;I=MEfpTA zc!Qbj=03+$Cv*o(Z)Hn!IDtyEj%pk~sv{APR8H^~xHN~iuCFV!TT0+j4kvh0p`Ab_ znzyH=1m%^Aw;ai)ekp;cPVm;Lb^;YSbS^QN;4H%l-k#M?pi&OepW3pi(~UNche~>G8O-S-aW^ zRLX%ZVr2BBkDS=Ky4YbUJswv!Yj8V(N;$A&sjffam3f_?zkYH2TM|4TS2o9zb^?`h zV8>ETx%-1BcP`#H?!%QHk1N|}?MNemN;$A&j!rCF_`5ZE7;&;08L~O2|RVe#}$98R@B1@RH8Wsr#b9gw0zP@1n2N6fk!I4t{vxmelVePH4(P9 zmo1$V-nve3ENLfDDPMCBdVknabQw$K1jonrQlV0*INGHIV^k`;(ko{{N-%Tk1ZQgP z1S-+Y=9IuAl@rb5$_Z4WE9+Hd4&tF6I(w1LI8IAN-%4d?;pJ>j37_XqaMs^Wpi(}r zGExGMRCdL(jHPlykgw`(Jky1V4;E~Gq;VOSq-AvdnUdx)uYN;Gf zFk;fSasriTjw>mFM=B>cKDHC6L|3j@D&xvqx882$-(Xb;+ixxVKsj{uDW8sRBv6Ua z(XRaas0!gTRU=04eGt(+Yw<|sgxW2AJHE$~jY_s(O;kq-o;tzqaM~YEpc3s@Uez3U zq;i5*PjfhdN;KD^DS<~SCz|`i2~?svGe~pbk;)034Gtb9PM{La8HaLYLqEQMk9^`C zXX~CcWcY_xF8`09Ea_DY&>G8O-m9O08Y$Q-A61H+= zW4*TOy2sZy2%|mbz~gacbI#aKpi&O(NT}vr+6s@ym91lY z*&mGrD&@c~O0`|+i$_8p_jp{{oTIiAs1ylX#E3>>H_VSXcJX*z*__F?6R4B}I})0M z$`R)v9*;}D@(j6=Pzj|{4s7KJyR=1Hb=>1|Wpl38PM}gG?1Bg@T$eQwkH?j*vA{W8 zToVTpsFVY{$nl-f7g|r;<8ftcOmn`Q4K)#{6bZY?(R4IGr5u`@)MKPPeuUkcGW!v$ z{a2~d?@siO2JUe}w$>QF=Fv=`QY7qHkMj889*--Vb-tZIrAXK!`o|CVcwE_)qk+b5 zJmS;|RQeq|=Fl7z5uez?<8jHC9zQ&mtH=$}KN`5l3E8aqv8~D@PMtud-?1Z69u3^% zab>gSw-cxo2|Kn`dHisX$Ca&dT;7RmBv2_5c2RSVAMWwEvRU)n2~>)NUDiAvKiuPS zWowN|j~{gcm43%AO2zTRJswv!YkoU{N|CTd^p79z@wl>C^V8v){_(>-9#=MNzDm_hpi(64g5db!9*--#dNgPvP$?32k%Oawdpxde*7IMx1O$csrqzC{~^DIhd9T0u?#RwWPe- zCa#WNvKdzpgX`g?a-uTYD>^Clz_n?ofY7Q?ID&_FeUM2jf z#_@I8OXY;00aSB%sZc2gEfU9WrFA*7NX3zC*-PbwN;tSbyi}-^gBFQ%P;YOI?_$-N zm#+Ku+=9;A>@HB{nTagZ+b{KIM-irSqH?dOnXocBTa*9lbm z9lMf4*R`d`~D#xeK6O z)yjKo2NI|h30tM=zX{qs!LHoRVCTM_K&437ktpBm>>lMnMLzBV^xp(sCu|Qn)&rvd zUS|=eiuI66IoRb^shSB?ii8~r-lnU4hSKA4WwXEEPM}gG>`0XFp?8nRmCap;b^?_m zVHX5%f_9I`mCb#Tb^?_mVTT5>mhRwqnJh9_bxLJZg7rsbte4X%1RZDvkiqZ`^}Uuxh6{sG;Jb zB44>umm`tv8V$N$_7Y+%NB=k|0*@22DPcQ-ic-;o{o|l}Jg#i&q#VrzDn-JMM0p%^ zkH;k+ZPh;x)(O>v(T*K!UVagQdPwDQWvdO#?;A7{s1yl1=AZ{V?(w*?H5M4V{ckAL z2~_$WyQl|WL~xJCmF=TFE03IY0+oKpF55~gb*u*-k1Ly1qMbmc9M~fI$3gdaT-mHE z?F1@C!j43F9CVMzC0}K<$Kzm~P}!xTv=IH{pnE*7Y_(x}9IO+l^gFid(f=ZXdpxde zjS`L@%>*h%!j3r@yIuEqT-mHKBKm8)zLmiD$7J(0Fxj+QO5mvzUi0Q0PV*JDGzT6p ziEJMw)f`Ts62aHt(j0g^u5AB$T{Q=uI-xqT!Zj19MDwk^v{ZPca)K`arUafkq5fb^ ztd`0NRHFHkVVVPvR8H`H#gxEPCwxt;mdXiKqWMl_ngfqiPVn{0l)zIb{Af@ul@q8$ z^A*lC2Og=M;M<@nfu~ORaj;q{Cs2v@ub);4pWA$qPek*q#Uqsyd}}YQhZCqo`+8L^ z6&|Ua;9GlX4ku8FZeHQ=Naciog|AvFCs2u2OLP3F60|!itp>9Bu5DTmJaxj)uc|qm zKqcD0+*>8Ce*6CUPxALKmWr<`$5Qzh3(LK5_tXi#kvyOty;vep$!=~dJauBszP((n zhZCqo(+X*+@JQu^e@VHT!wFQPn_G9+nFr=CJ^1c%sTMwQV7~uqJ4ajlWw-1yx%g|@ zO6%gO6W^Hszw+14+4T(wRHC<-GbiRwYmP@MCl0#j*!;xbZ8so?6R1SLdxuZtS5IAG z0D(s;C%(Vl$@!$WtosH8D$&Qic0&I5DUZZAG*$b<$F9%8b#K@w|KvfJ4IuC+t`oC1 z*(2Y6t&84(KqdMuckSI!Dm+p-vElAB^7U8$`G6cwpb~x18T&T0E*`0zm^|f!`E@71 zeL(9vfl4$ZG#w>)q;ldd`|XooGG+4tIh;TxnvtL8@E+XjrGxW>XTPPg5~YN%QBG*y zT=3IuKk26RagHv{2S8uc3E*`v%<-)jKm7Tc$JFL zE}NcJ=>FeS%fs1{Y>sNOIS)z+JavMznw0R8FwaFdmkN(mPOuWCIh;Txnt78Fc%*WI z6}6o}C7O0i2|Q9c!P!|mfl739>w2GiD>RQ1Jc{cCvpFr56R1Qp(^A4~&Q+KQ&QVeV zk5o=@Mb=KB63z8hN^tfn6=#{UE9X9y8O7|B%I71qIn9BmPSA$!1S-+2F)4vZDkqxz z!{<3WA)@K0GzT83oM8W{L zni6>G1lQE<1S-*7Q>O$Tskka$_EI^)EJ$;BJy0nJ*9j?sM=B>OGpal<(KnWlp0j_x z=Q-Dm`!}6s`*Q&OGh})0{J^I9SFYSF5?d|ZGN1dC!=vp_CsYYMqMi8nD|gIiEjhn- z)+dz{sMsH$8)E; zRGa?jhbn>`RXTMHJ~0&plUA_<(0~bHGXwY{*_xV8IZ#X zRHA8-v>rZ6yf?SqaHD+TWlL*keTs!gwiEtjO|^BMKqZ>5xum7SBb5_;!6zl~)CrZ4 zr^2eGasrj;HLut>zx%1F18RQpf%EgzpSxhXRBCDct(|Otj;xx4=hLKGd8Lc;gO7Ro z-gbgmb>g7^xgejp?LEai&nlzc2~?sfZQ53h2C23<`ojF&8+NbEm6Y&V{%D*r_Z`&Kz}%aN;EB<=Acca+W4t6^O4z0D!rK!-p@{q zeeAgWg$rJs-cF!WKF$C1FLLG43ddM_+njClonDw7OU0;6bKnu}#2UZeEWdWa)B&T! z2~?t+N4uAO$?}Kh2jBd9ELC$o@F=bmD=gVNpSsG@0d3_3DsPdr=H6CLeEXVR@@ap* zXFv`oP9Oc>Tr8h4uziTL)uOiCk`-&++E4oUy*0J(3|Z9D$yD}<-Sj)2k}Vd z1fw#|;RGtt&25E8DkqpXX$~h)iKd@Yf;~&An6KeM4*z* z_w`Z&Pn}?mX(v#L=DaH<@JQtZXM-uh{;mkl+hw!2oDz8I1XmXA1S-+&k*5S6shnu8 zhZCqoGt<%>c%*WIc55e4iDm|;1Us8jaegJ6J1HrFr%rHg*G`~{wBIXAR#Dxll8P%N z*^GrW2c9~?bz3`uN;G3ECA{ohaDLrRpc2ifObL!&QgMDQn=_7-z*8rm&CXT#lll3I47zSu9A&P zk+36?z3)%YP2`J?h&k{$A)7OUb^?`hU>C$Xw|%60-6vLyIq-N~*{qfA1S;jg7BMn8 zv{U!#(8BKVxUyNn+X+;Pgk6+sr$bNd-uJn?Cfwt3WpnImCr~L8b|l7*{oW?sBd_Un z-Q#g(vsShfs1ymiAhuZbw284x&x|?ncwE`6G3^8@<-jh9oE&&Ou54DC$e z-`k{fO0TW(cwE^W?b-=c%7Gn;EcAJYKF8y6WwTbc6R4B}TSOeY9mXymk1Lxsrky~g z9N4i`L#y2OkUWK|^6GAAg|lxQUT33u9qpKN4^o8N+?7*~kwyZQB4HP$`us*q^Iw1c<1q&w<#3z( zc}&}v*jy`#$zk?VtHKI+|z3(P$?32L0qxp$2;df@`nlccwE`sX>2D@ zDH67bk+B`G8|tjP;%;5{cwE`s!E7f`DH3*3stxYEGT&mWQ)3Q19#=L`Dzp=*lmj~w zW4}G=uJOJ8J~!sT<8ftk&$FFCr5xA=@ug2cH*v`ikBB+&cwE`sS8pd!DF?QQk+GRK z?ACqxnJF;`9*--VCnMSkRLX%JiP2x5d1`mjvzNzlg~#K{=J|4akR!VPvB3**p5yVjvUz@@oj|1=*aflfQ73h-cT*o&9I89KZF;4vT1ZLaIa=p!I=l^0?@)A)`U66??+7;&s1yl1mTKr{#ryhV z4m=)LHamLl1S;jgE{Gq7{?@xD-Q#g(`(9r*)I^|CBk1Ly<^mYQ3B4HOq=<_apj>qH5X2-LgK&2emk%(_kx-ImHdpxde z_Ile1REmTh3BAFrdwcN)v(n>n$ydzUz%&x_rJrt|Ba(+-IBoLglt+tIYx$-TW{@r{ay|yP$>s?BsxXevN6Rv^~Hnp zBQN-C99Q&*e%ICZqEBYECpp|B+X=6E|EZTcfl9ysed36iJ1y1TYtG7_-g&e#+C|*{ zwgdB(&-h;L*$?-~rgddg!ZZi5TqRq%r!9JXKn^^0f;LQZsD^hP^{=rW^nqx#?swm| zU_cH$QaM4L(i~2p60K4xcS_)q$_ZLMCA=O(R~=dQ4JAwoJj&t3_ivcf(1T8(63vK7 zbKsH63FRBygHE6lT^WhxT>#A-=DBQW+5I(9bFlX&wvJ-aA0OGe8{^Xdw4v&e4ZRlL zZ1jilKe9{ZglyWaoj|2X*pZm>yBTM6Hd?SC9zXDST-mf+JAq0$up=?r%Q4{|k4rxK zBWom7LaC@nNsRT{s_Pz)E8BZ8Ya~!95_YUd_UG^}y`?kGh&k|hT-oYb*2JulK&2em zWsV7Q;PJS!eJzPO1`?>013TtW&AVY-#d_fJxUyNt+6h$3fgOp_zl3(WJ-qYGJswv! zt7|)fN|CT5F{PIyj$J$+mwY-(v=gjql(r-^2h~<_4x;k7vekyHqH)#^Bv2^_wn`OO zh6%k9(LEklw#GtvG|*ZykU*tK*kumAi7n>9<8ftcOjD2k(ODk)R<@5AMM{sg#Fb|d z!ThM^AQrS#vYBbI9_4unaaj|+Z-|r*1>?Eh>WC?K$d1+2@s- z6N|V~$*zp63PCGM#VQK@#y#kSukF>Ed#O;7ub5H4cxbB$)&Haa{ONE6J$y8nqcc~U zG%UOLiqfRv3zu#+UBAEen3uwTov3=Mgy@3!+duvhb3FR3L%UAMMm4d;O)*C|e<1wV z3HQ_q(FHO0J-dbSXW4fy8R|MA8`TS)$77E0UCZ!aC)`sfL>I&zm!1@J?7q%z6HdrR zb?)j}XuE85lbi1h&+j_no;o4AAp9!T3E8OZ_u}tTW2r*cA@|gY9vuiPRo+0{YxP@P zCEQ~X5&Aq3HcC29SPw3?@m3?;Qzw)ojNL%kXzw^- zBQdB(rl`-uya`ucPPnH|6m8Wb^1y2jcASunD$pTMoP$NFN}}qi68(CFwu(fUgQ`av zOE#*|3i0={bxXoMbwYGOggy`Dk8@D{QO1&uD)eUjJ@=Q6NPq(m>vQ?>=aQt%*o%wMj$EC{!V#amLmT zR(`z7DAcsYwgHn$8~%8=Iu#WoP<8ISJ0UIA+UQVgouSjxN}&pag7~9T67Bl#y+8Y` z^JAZxKT1Mo<*x5%t+juiea?O7R@MLSe_J*w-rBsX_Age|j?whB?_2)#$p^+qpZ&&) z>^<-Q=kZrBek9mCZ+U3^x*Pv>rAzEN_42(t?|3$-ww-_9-p^nCOeF63%xv=BZ}$+y zQjUGEKREl1O`i*d)w2GE-B0xqDV7MI!>XOB+HO@|wdd3Wd)NOqwy8|u2`zVoBHg(7 zbWaXXpz>|%R^?mk3H9Mup6hG5Cr~NJ@%7d6XV+#QeExqz?=nu^s_-bTC)V!%o4sHD z?dN-1?g><}s%rhY&EM)FENk#cLKv738VM+SJfhd zDz#~*Ug(a8wKjM{?dRMxqu+?sCGfO~v%c`a?5Cbw?8)H?RH9#X(x!b6?fL3O-5h=d z{n!{i?$3coaXn!(*h4&e=BH;5e)xfzRHN5EJG*(?CBZ)HzF(Z3_?>Ugy1k31O^|z- zKozTcX7M#I*tD-pR8{my)g(&o@C2WvvY!sXS3`hbK^pZp_4qov+sId^MeX^uc}XoU%Dz$z~4@?I;rL!GTV@ z>oCC{EP}H{m+&ff=V5|aQhCB6)hC%oJ3N6(w6(lG!!-!6vX+Na*f2pXsXW06u-kG^ zpb~A@-TL&}s0xo%o>0DgW+)PQ6<253MCv8N z*+43GYT4}hU4mFrQAx7vox0HuUzI1yIXr<%Ik>KLtKwQKRaKon($!M7(c!AyCGfNf zdSRGAm8uG-fiA&0K`Kws3xSw(NB_=`eKGf_Y-_{pA0K#ZCD~)y!vy=Ir@MrY>k0PC zVFDF7ZuqC8Jyltbu{=^S+lSjBg4TM1_1LY-S7JoCcRprSGgnn6*ekm^)K?IwXveWz z?uo`aZ;ZL`D^$MUx>d0zq+(vlZmf?+4m@q5JQF>EN;GS;TNR$93j2y|Ms}D$C4ybM zOW^TwWtV3O>s%_wU|d+aA5_1L3KP?$!0Vt}EQp?_WRbgbOCK%@+g$k0)ev)mDx&fl4{BMO2Ib zed(^r3A=Z1c#n@8^VQEw$^_T;*p8Z57^NJkh0){Vl8+NWnc&(^wGcZ-sjnv9(l%ku1ug(4s83k+O8ZMHcLVdJU(v97fzYOgjE<+ zsU4Zvzi53mw|PZ7g2%^|Z9SdO)MWxy>h(-i>o(?dn^$4XQTezrUwk^Oc9aQD`mr4~ zu`o?J!rVpWE$hMvicNxP3 zDn-H;QSUz9HBs+A-s9tvkKLzCsCKES7NXvLyvN6l`Rd)LOsJ>% z76@BiRqsBr9Z}`u%9fIQ&N6{Yk<89J+*;nRyvN5)`SR{lCam_LO4Vkf-hI5s$CYh8 zop+xyfl86E)wX)~@g5&H=Bsy~GQmz1tF4K8_wgPdmwfC#WkR(}MYRz1?&Cc^Zp_!W zU$qI1hg;`8`&Dd5RQb5F{q9rmS8W27KF8K@ly@IgDPP{Ns?pO&Z=QVrv@6Hj$vC|# zuD7|5uB!jM|FrW({Gf+GrAVAntLis5Z+bw)&zuo+;86~U(7yEDt=h2=s(K>j@B~jF zMeuoO`RJFQxPEfQ&2L-r*}Y1(KOt`~5~vgjTRC=&?tJ3X$t`F7^u&97T-n^W4HKvo z30uT_M?d;&hgZ(n`qq#GkB=*R&3y-1CQvB{wsNeEs_H+c_HW36$Hz_f-uyl15D`=< zMm2K?{_n)e&OrT1Am5FN6#$(QSpz?8L+X&|8qGbY=a$twPTDN&M zXI|m)ab?>~te*%M2~^5~tsMK;ZGFst==3Ycip9N?RM`5Qr(acd@#BX-DMD+|dpyz1 z@e{iieon+IdI%~}B%XMys%k1?bE*oDk1GN?61zYAuKAb#VQIrxB9(6k_n*TADn-H; zv19t;Q{FiL+%+$ldyjIUlFj|@Fo8;uutmIkdiU#2nZJMGS3(XvKCW!;cZUg7%7HE7 zw&@$+^-CL%egF4D4m>`tY`)VOCQvDdDf8X-s&cH51CNiJ@`Z1+l%q^og+Z0tk%Sh>J`CTUDMy(=mBu3z)w+%OgfU0uwA2~^5~t*WX;o4XU{E*>9O zHrK>q0+n)Li&)D~guO@h{N$e;|HbED9Ijneb@$?*oU=7PPizpj*1}&?h_Ia~{Pl)x zJJp2KayJKaS1RV1Y-?Z7(^~Io6Bawqt7bchB?6Uf)_k`rJZ-{yVWwUfCQymKV%x2= z8^8II9)doR%J-E&={MVfr%iAT?pEaqRH;q%Nxw0Is`m{$&r9PR?@UF^Prhq*?Y%GW zX$PJdwCp1fDiQ?qLFzXj2xhoOq;a5~UM>CpagFwvo+O&iamC_xi+5-?4-- z(C6HFhdX-RO@upo@9~6eMmF5h>uw_4(YFay`kZ@p^;LaG?>#=QK8F@jpU8YAQqd;a zjOH+bN|CSwkx!Q1ZJ0o%NZ66!r0+dGu5A7$V3#zJv50A6K@` zl6*og6Q~pkTREyJcf#J|OkPp_m1BF(8kHj zw_ort;Vim3EfJ`c16w)vuYb?JrGCc#0>XQg1C{LQ^}n$E#UqD9jxvEtk+7AcTHk!e z)swgE{Mv^1__(q!{@z=b&%5gJkfThXQY37pSX&$&x?%FG=PraCczj&hKlaGmm-AaG zMFN#_U@J#8QjS;KD0+{NOTN>$#rJW_gesJZc0jC-j-Gz?=>mnpLmatD_i4#c+U@q9AyHPB4JyK1GkR;z}oP zatsit6bU=FBdw^A1CNg@TPy9k+qa~4ln7MHfvp_dM@DSestq~t__!(GEVW~RunL1J zwIdU&>Z=X<3YCv5+j@5&<54D1rC!g({zV(}xvg`?BY1pV*)~h|F|W!5D&@ddUsa1X zcjwGqJU*^$oB8{g?PUU$a$tuX>$V2xtU)|Ju54Si`&j2ykwB#!*dfPMIoM0^__*ZD z`&E%pg;LQDh=r-`iF5WuR6ee3^&$IinLx#OKNF`s*nk1Jan7G zPgk6cE$JGC$H$eeIjt+)Fo8-rur(e#bmi0<4DG<<JZ*xN)~(7DsC$ltVp|pIOdrY{R?ZDSLR< zm#PZmESqY(Iq-CedL}m8;fdC~Vtusc6;JMz!)8f1A<4FO)Xx(e?ZDF}Y&7#3u342Q zP>JSQX1C>dr1FIIbUuqVb9e%kXdBJKiqbginktq0P1o>fyC?LGmRp_S*?pT(4zAA4 zsyu;8w5rYDG&FMHk;)TlQ*nKSSRzo#RPAI&SBMMRYQ3XfEt&|2xw;R#ftS!uoP2yd*X z+CyB~)~5PyqA`NR@+xRf0QK|scrA*1M!NQeH4#6rrkrHTgw-5WiLUpOiFO%XLFHl8 zWHYahm7|X931(>Mt40oA398|$6j$R+g&MWvuS#u#V2%w}g^GNo@rXMPwQK)4{;Jr! zM5q_?&M??lQi-5Py6rGeSQ9C3*l~`(Dpr>W_O5OYKZ2-Yj@TnzLaT@~E^FfWtMUZv zv75tJg-SW-kuIUyd8(q8>x_H+Re3@=I8(RRU}Jp*m2%J{p{o2;?Hl)gJ-pka_n?xU z-<9H>rY50hNO*dPK#Qm>hbpVKkwb5WVe>u~ZwxgFa+HYt*4EVi5>R>ORN@KVM6(>> z9X#b&%S6f%JXGa~{H9)IgsuqHUy;N2l_TnwXM!9hLUgFg;>S}Tl|Y>LiFYpj`RH5p zTh38mquNz@!eXyq`;qpq!?X!h`rPQf%Ga%G`|`oPmpptVSEAY@aWwqaPnS6F6VXFH z2#rS`!A8s51ihf#TRzqPbr{9PLk@^?JJdfPzv?O45yn}xjq>`V|Mp~0%kiWfnP|+b zMmt(Wo@1&?x%>JmdT0lIMX&b~xgDlja$}R{ZfBO{+C5>t-t)US?bf2=^ZpUkdalPH z#ucG_{UhiJR3bD^%*2K1UG`4Vm+V~`dFb7}-#KB!k0!SIYXABXd#?1nJzpZ0`rQ+< z>4jke741+@i-_--oM>;+d5@19^VPjuCVcN=D@A_?%9nSzGGXlqs#I+z_Agpr&FL#tKCW!*-Mn9w2~^5~t?`KOm@wveeB79?-gnD{ z&84tA$9B|2e8+^ji^|6(AG5tos6uuhY&B!Y)YjmfHHgQ@jrr<5u}r9^*|o8iW4CsO z4R!`RKCWyN=Dod4po;UVz8aKwABdDM?8o+*dtM@sA3;B3v`gjtsC@m1^+r#M8)jep zyj~ym-rgoyABkRK4K@kiSAJ}cuPRS4d%7(rR;(n>KH2n0moQJbl2JRmHj%mno-R?3 zd9&r7uy|o+B)aG7OkDM%Y*onqNTlJaJmL5Da#g6v#~Q3p`ub~b{vtv4p*J0w-Sv{s zhBw3bYlvo5`nH(AXp;T!_x;!S55ICS{ANx4*Hn53>!e+m&u_YO>)gi`QOeOI$nFW* ze6Ff4Z{)z^Rm#n;lynI^o{-JwBC655tqogtT|QAP|I0Sm`kX%O5_l9BLN=dA;^!^L z`6&k;ukwA^CFoI4$mVkq+ee!%{(J7+8rtDU4O=-F*)D-caUo>$c_dD;9KV}#;PEQo zhh4(=kEi)u#KQFVE&ho%w-k$!QVwi=?q_0?z~c$od>(VW#&VpKa^UeQ<)9b31RhVw z=5rCvHAriH?d=@(I#(<|OZ?1lj`_aB-`)3$m)5Yp&`sOf8@OVNtzhF2_pi&NO5o?QEKmPjp$F6%($brYlm2JOA7=9UXm_Vf* z*nwC-`=vL{Hy{4)viJD7viWtvVFHyRVT)LuUi9m4oB#bE-@oBKKCWzjwQ!g~rAXK! zu9?2+!87N-^_P<{+wu6gvia4*VFHzMV2jv2efufz*tq-l>q8DaKCW#2oh`qHSSC;@ zhvpl^s&cH51CNiJ^5x$hEE85?P^ETcVr|{}YC>P3@^NKbAJ)G$StL-U@yJBAXk$KM z%u)HcvTc^+-^?r%sFVX+<6$R#Td~YtJU*^$ey?+wK&2emBGyK>1}CgRJU*^$TbtpR zT!#r%%7Jaf>e|Tmk`?w6JU*^$+hh2>*8u{Ra$v`*Y=>Lc4(B~Su58UW?R-^r#p-*O zY>rOf@q?Q?1gaFN=l5P?thJZ9T{P5Aj{Gh_K83dl<&a9Bv%7|NGzc5>@Vl;3+1P}A z@K`zOxSp_H@A(y4A4@hW+2yKO+fw;mf;M&Afu~Kdj|~&3MAIW(!mkg%9?9J$@F=b) z*fE9)RHEsTF2Nj?%FldqcL_Xgg0(qJpb|}wh}f-P*wF7rg?HR--G?`W^c>&61%n-O zjP%6of9?PDMn!lV$P==83uc%=rAXK!s?n2H(KA+=_xQLmU;TDXnegw*V22!wDaS@^ zM~vm;l8?7*sv^NVI8+OSQwPHab1`M9z* z6M0*OdeK1EB(m^X6}%d1kpFvq$%tgk{ZN~H=_JM^(f Q&=Y=dFIR<%d~pr_A59&cvH$=8 literal 0 HcmV?d00001 diff --git a/src/wroboarm_22/meshes/wirstBase_link.STL b/src/wroboarm_22/meshes/wirstBase_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..0e2de13505b41fccf08796b9ec470ce260105d92 GIT binary patch literal 12484 zcmb`NPsnan6~+&wK?D&96&4ZEeF++dbm{&6I4Pt_1`Vtx5)DF>Afl9lzVZ$n1Uaxt zh&9kxoJ2^F^?u)dCwI`G;8GwGcDn=@p~MU)LIYjT+WT4iS!eHaY>*(V@2vG(&tCig zIq#jO|L=cqdb+d2G~K*3O@Gb% zkY(bzJ4zHYec^-O9uh5T^XcDgKm7A=RgLRMOGNP??~K?!@Yk_MBdjKm0{!RLetYzh zuRbzp6+N}>K|t??JHI<1Q0=>Z@UXZ2cVGO>(Wh?x$n9l<9%v}>+5i6ZXnN>}0}Uk@ zwO&UawKkg8+Wpc97Ei4_#^3tw9CKkb=85nq(AL^LMzbgAsTx5*V?5?_0#TLTFOIo; zs6oO;_Iy0%rXB>NSjTwG(JFWlrG$;m`Qiu$!Kl*0d-s?Nt%~)bgxTA%hOQ_^)fW2da*b2l zub#2}YWL9(JmCA)WB+>p7B**oeSVJTL&7~hA?stV5d>EeHDd10h_W*v%4Ur3AF#QC zN3AOAmGD*cP8wz942ZTqX02)jXFh1oc#p<8mzEJMdg;M?YoJfXHHlP zx3SI_2}acxw(}zq7UiBA<$0+W`_&?W5kwR<7U$IwXJRJgdS%6#hyt|&w)!PT`+Fp8~;y(HCv_QHcGCCqMn$r|BNwT`wYCIagYdt%iHf_vAn zjvyEXnmc$;(1R!?xCgHji~`Md17g~h8`C}9ul&3#J7-#tv0wEBW{DECc{;BXh_Y54 z+bS4qgh%t5?azlAN<3N7TOZ zDADs<%{vpduPzof!oya9Fx#I;Uumn9h$zew?n+&&!lQXja}fFIP?vqLkp?)oUohDA4Tnp2)MW zyg`7l`q~@hY?j3PpwFecqF~3W&1247C2zdrpmIN{`&Q92Cv**FjXA-2Y{cMRQuDfp z8YHZ*26vx1!6-b3c7MN84{RlDY}y-S>Rsjn!6?}1!+8Hl1U+-YX3XFguDM_{3O3h@ zkH>-E8L+q01DizI^QBdZV5{Z?>+3bJFV*{1?1@FAogeL6hI(Lg9+&4bcW13CxipHI zVi!$>R?G1Kv0Mi|h*E;%-&d{#qiU<#+)b_GtVYy~@N>7ZCzWZAlPZ z&Ao=s5*`1v?@1-J;wxC(y$2EK!0{)WB-) zTLpsaJQMOppTnaydBTNr7W~>o}Mj;x1N98teFWdY*JJibe@+Rc(BJ4N56}=dQdhDKnp61&N?6~!< z5+21~A8C{sF1Jb7n5LI}w)cd3%B?1~s?xSIL>)6C>ZqgXJ>uhG#NFOk>e0%z4?Duw zHriXqfIW?IO06my+IA%@+IOG6awRmXKKGu^tx}H?Rd1^rrB&+Tyh4;_TGJ@=wdC~? zOd4e-=3Fhxo_F&q`bs>FsN*!&2tuQ*KI~UP+<)otjRz0E`tdbW9#FT>w_N63)|L;zbC8{jDiMj5Eq-jKYHcrtDpa#&nv#s zAusJhw70uVU_Ik*s6=U1BktSWcKcn2f82JLYbZe*8rc1K-^~O)nin>oqmHwut%ifu z;(G}_8dWQAZ+D5XK8Y_AO7l51zPWqFG;Y6g<*Mc?ttvF@;FpO^AaDI;6l0L|NL<_1 zzB*_w)`6&^-M#^20zF!P8IAYl(!xgE+TwTfy-@mY|KwN z=JaS(siVCQX2QnV-xQT7dX0E^gZ0O`s|0P0OFa_Tum;grz6O<`&GF#B zfXD=Tlb#bX)S4kYH4)W8p^Ty<7M8L}Q$I8eG+TVj>s?4gGf+nV^T` zK|MZ49XIs+h+H5TRqODRs3)u?JfniBdO@U94Rr)TyN*YEd(DLPD9_qN^Eq0TPG|IJ z5VT7j3pW7E8+V38^EouqEh7<(Vja9$W&$m#chN-iIV93;$y-8#Q5+BcN|p)qXuXRj zn$Mwe(0iiyC<#VE1LM!%CNe<}$C)&DqG|f4-E&{H`|dP7zrmWr*GzmPp^dL~R*!$V zE8k9(P|t`c-}>ILJ1G92rjKoupp7W2$90N^66zTdMFTaUWz+O)`#RTh(JoQ#`^+>c zQNP3ax0g(`FP=`E6pbhCdz|KKQAYb$wo->_1d%;G5q))9`s(GK60{Lzy&zwTEIJ7F zjEJIPHJtjG-+ybT1Z_lNop3JrED1tABO+S(!WOC9@|SI}^x=5^1AC>pi$ zQ=3<;!wJ%*j`ABq%q#Sj6SaR9iK04NQi67g^0`p!P(r`&W}^KT5;M^>p0IDc znyW<_?c*Nv3bWmb?CFW&P|t`c8dw`Xf|qY`owIj6 WFQRNTV~>g5=VUTMJ=9a8XuJ)<^e)~2 literal 0 HcmV?d00001 diff --git a/src/wroboarm_22/meshes/wrist_link.STL b/src/wroboarm_22/meshes/wrist_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..0e6b6ab511e886d43f894b64b5e3ff6133b1c466 GIT binary patch literal 50884 zcmb__4Xm|kb=6HNkQi#Aqomei0cSp_MW% z^(7<62lg39lnlMrTSaf7W{fk==2FA4^s`X3(W*UG$qK_BqR`tt=A-)INQX71jJx(k z^S{N~?S5c%3#Itg`%Ol9lOesc!wjRM=cPGqm3l6XhdEZZzp7S^49(4oM)uMuk1T(# z_G4#OGGH?YuVxfV5tzZn`0}-*qo2g<#^pm#{KVOfge+U0RTeV=6$LRQ3 zt-X>_s2{ofnX}*h(pzeMO&RG?>|%WWowuAl1CH z8HrNQR~YGe&h-b5U;nB*-SdvL-KrUb`|Z7l`vGsG)br9=)o=@%A-&^GjbbEfKIb=1 zaXR0J)A<_Jwd?QIlFGYztELQj7mP`sPc)KYRJ=Fr3=>A8VDrSW9)GSd_^w{N8Kd?` zj_#r#%>dK8C`W`R&WeUTK<{Fw-dX#w#*}dvgL_5*5&OIvzphZ#nZq5hS| zmv7>9#_PtZ)D^#XxLxdzHC7XbcQubqv19L=G14>BbA+oIg^WFM#)wmfNVbTtU7RH| zMtYJ#mf0NLvF9^Jqv9l*X{0CPZjWyEBTi>)RiCD<%C;vXZU(bfB_mPpHCwu>!B*^& zA)7a~86!Pvm9we(uw>w!_9R0iQta(kC5o~AS}@v^49)DiMl%vMUy0Af$;{jSyZG!( z#$GSm{#oy{RTGBM{Ar|r)_cXkxx=&YU3CKxk|@83EHvL+Fxs=h*pu<>`~KoL|Gm@y z^za|Q{(nC8-w=1P;~evS?{;L|b@P+|`iD-x@|CX?!yam+?cMb-!zgOlCgX!Q&)#EHi@FFZ{THH=c9`ZU;P09mM^e}hepM{d6GY4>0M`M3X#(l9FH()Q_dm|+w(Y?CqW zCF#kyvUziwWz-6zP@?En_(0+Y_lH^b3QITANJs9qiADp z`ZIuShEde8O^u)8y1MkbvM1xx#u}?n5DqhpqK0iU9)A<7{nD%5o{US|x3j|xqlU5K zeeOox0G@rDG%nJUaYL`p^KFJ&cozLA#$DGmR;6K7#-*(>tUTXl7)1@+?D+?;Y0fXx zlW}R|#dTF0+YFD1bhO>)|?NiN+(Bthk zK2KD8YnP$@&;RQ~`;WcjEj!v6k>26P&-&6cXZ+2l-oM9dx72VfGK`{)mNBDN7{iWp z<5r^H^r_G9KmT37=+n9P#A&OvGZZQrf8pOgy8rQ?I9bt1hEdt4Wk1BIJxUs7yR}P( zJ<*S1yz!s?POaO+45Na*GK1a$h(b^I>rSZmd=laXN*ZO!Zo z!#e{}JTKT6+HUPq!=5<&c{fjB!NiE^gWcCIWL*;}pM>c@7^?P1(x zD7#krkqo2MLaz2Y2d6WbD=`^b$157iFp3&p$IG5$mDIjV6f?-{$}_uU*fVD^dpBpW zPZ^`g_KI0*W#HkS;0h)M>=t=95khEZf=p8CX@&WAljC4>7R*T7OE8Ag%q zQC~9bAu1W%vAG5gGmIkJv$7yAa~PVEB|1&$Pqugo#RqQNdoxLCs)|kLJ8=w{|&J_AqWTv?{OYxlz%o z_0xbERV~pxdj^@!D;io?3x-zA6%3=;Dv!vr=k^el46Wl8jbs>=|xwwAojh(8kn#4WEe#@-mg(#w#pu&lA+yhrRT{oifqs3(a~Rh`|k3VmpFeo925qmgk+EB0 z7{y2*d*jnj{?eD;D2*E*{KnIZ>pxo>_GDbzUipK@7Q-lN{G%Ux^2uku?iZ!;nO}YU z>94>4)1_fg#-;5QGiZ?U*%gLS)cD2^{q~Npfk8A`>B+dXy^aqvjAA7Afh`^w94GcZ zJsFqwbk4UJMlllSf+Ig@ya2Puc~4KqrR_B~FgV|945Ju{Yl>@YV0=ARAy;F1GA?be z{G+2+VkL6j{=OB4QH+GlyLVXEWI*HG40|%J^mtFy3_|1Ftx5*^Q7E-`KlTH;ap9h$ zhvmt*w7tjlSgkUQq6X$;Kh6hc@WOL$PsXK<6|?8+A2hZYMo|OnYCo^9u00u- zHdcO}^MeLf`zpgIY9KfEV}2n1r8CN&j7uAR*mLy{8e0sbsDVD5k9m%9y5x#aPsXK< zooLV1KWJ<*jAA6r#re2jVZJY!Bk9Sww6Uh@`E$@9!+WA*C5n--rq0KG7psu#w{~iK zl0lnKM_8r!RE?`0E4^|gV{u`#)6^Vo`p7Art{;HIg*}? zOIsry42;!+VH6{248_n47Q>#5OIs`E3WibC(7F_kvYRCzOO70 zLm73Mo{URd8GHr9C`M8?W8OI*+8Ij2o{URdJCQWD7)DV;yOK1J8{FYa!=8*wTRW;W zINxgwqo|?XRT^Bk%#rkDT-rDpj)ol3`N7j*!7z%EVBb3Ah|Uk5IK{9h<4R9YsT^IN zQEpW-bPn#-TAqXI2k%zyxlYLQj7yt)6<7Z-gF0*7kBXHjYH+V&CiI;1&QJ_{l0ln$ zE?56yhEdeup358=7`$6CN79pVX=CMAjtmT*1ZxbV7>Q>Gb7Wuu%^O)|lsy@jHu_LG za+qNhHFyRx69xvxiFZ+FZFw>-ZOldG$YF+2)WBR+jtmUU_bGEEJsFp_Zsgi`$?%@& zSczgJ-bt7vg9dLg-rMa-25mpF_W5z@y*(L5QNzy+Bg1EuJsCIj@XYzrCp`D`KR)Am zm$yG~kDouVc5dAI`@iS*li&O&-*o=Aul)A0`k`}>jQ{d&f9&$l{NT&3rzaV-KlbjY zoWJ3_e{_f6y@K&;V7zRFVH6|jln~>uz47Un5B&7|O2eLvOZ#hn@8ixNdL}=oei<}g z4aWT|45O$q-b;P}j9>b|lP+)hjn9^bJsFqwBme!^_IRmP4}$RrRv1Q6L+6JWPyLP0 zUOe=HC!MAz%wlQSlW}QdwVYwqfWcMrQZPQV!Z3;&IzPntz6b6( z{}0c2UTN5qacLhNedOLd-~BZF{61)W2N=J)!Z3;&er7m2`d+pQEB%vV*pqRkcRcok zxq%$1RwaYEL6rKzJzxD`ZXie8kMd+(+RTk{KA0OGU7a{5iPC)Nd5|;6+(3?aKFX5} z+RTk%U2&B#H;^L>hEdee`GJ0LB{DaVBgL>MF<|KYV2&&pMp0wDm+1UpjugY5j7wW9U#((pAV(Gqqp0Cq z)X;OTbmYiodNM9;W$<~QAJ>s13x-jQq-+L5=g0NCaeAgFiC1%}R#>&TG>!zf1b zt#)8AH+Tz3PsXLK9TgfnKdvK377U{pNqeOj%njs7G3?2>wD|;p)enZwj}zp`f?*Uj zcpn;`KXiVaBS(s1PsWuV&q1|{xxqVD#!8g>u}4OW!Q9{-D?Q1ejgjVg4hH9gx#7{( z$$6eAM$)M*26KaVtn?&-GVL&W{V;vC@-qX=CN{ex>|i zZt#vZXBfrzb-xmWxxpJ#dNM9;_JOVJ^JA4^6eH<22!_s&OWv{4lW}QtE;uK{d@wh7 z$C@*Y;``W@&N(LogR{pwR(dinZLTS-e(k%=4c@Wl45Roy*A&-7pC6aJW2GnK(jIby zpI_?;|ClD4vTuE8?!Bp{OH%N zXe7fZvYn4h4SR@6#*;qutn(M$|L?Kd=lw{AQDi$GmkfJ|O2)-+e&_kef8w4Mjbs=_ zw)1hRVGmKs_+P(%=lR9|e)5V&GK?bI`MA`uhp1#+-1*&@i{W2?PIE9BMnykXvR17+ zu6`(P=fXrJ=R3!muaf($@T6!7z#pt?3npJsFp_ z*73=5tde0A8OqHShCLaVw({``hEZf_k6B^ZlW}QlU%7%|6dBqpR~U)Xo>))C+YEac zHyJt&Ry2}f6xqlPea|D6+BM^f|*^!yclNfiqLP7}Kf#H!lIO`Vifrs-H783Adx%N~_O270I4c^- zFp6yKu62`IYS=?mGO!1q;KW(cNQO~lKXJ|4dBd3XHS>D!+6*6ZnET%%|^gAX$r zMeW#2o`3Uo-}BE!FcfIo&W9$r@!)^KVKU5 zWL(-l;i$3AXcRQs2IJA2H-GDa>;LdWf4wy9$+)zAQd47_(I{xN4aOI5e)8G7>rcP; z@0Nx=8JBk6WVRWNf=1iW7{5QrIkzX{%I0(Q3Hx!7F`$M>!9YJQ+z)7!C*#sa|M+|^ zjV(r_hOuhKW1No*&xcVNmp0~!&*!DF&1e)f+D6YG;kvr?y0RzZ(#Cq@^Lc4(Ga3br zwxRJL^W)O_VNb@TjlALWd1-7j8U>BEq48ek`K9yRo{UQy`OoL`9S8>)je|=aBFO6+RqoC0?G@gGG`|hRpU3)SvZS28(J}-@JMx&t7HZ&f%52wMU zPXl{0Zt2DI`8LDP=ghRh9&Thue^?sl>B+dES9gYOhFZ91q_)wIhi}tZU8E=DmR@{b zvdz#)mqy#1k87Iq(y%Av($-q2erz)ug?_XRjkjLY+P!2y?8&&awWiNOIKXHWBDD?1 z`>!bnOT(UwOIx{F8rzIUL8EPGJa|oeNom-VacOIhDUEGLqoC0?G#iw`Do!_GDby$NmiYAY(uck%FPK zw)z2$@?>1v=pUcxs~=xx&v`m87>ydnsu_=QK6q#FSd}N^(#AaTiM}+p8I6KQ+Zd}y zxUP8jDGhrvE^VwgKGB!PHltC{Xd4<2GCz2SD-C-xE^XuupXf_to6#s}v<;2-GS7Jz zEe(4zE^XvLpXh5Z*=95f8f`=4P28_|r>_0Vo{UQy`xu|-OJkeSC}^||jptv}vw`=- zij{F`W2fd5z4pW{Mx&t7HZ=atZF*v<^TVEuTYB+Czs>OTIWw(oXx!L*Dl#hcb|6Hh zDaIPU=Y{hQw!axj+pS$P+LJBGNC!3Cs?fNiRnn`cJAY$r*(!ZMs?I*Im`R@&t;*ge zBUi~-k8Bc)J@@K6I9C2H8QI!>>gm|rt*h4;Hp3k}TR@EvfVFdxHjDq(kP7mW7I z7?_*!cd3(BC8JSbcb~R2YQ3sYWs9-0N-|S5AKn=@vbOUXElGyPYDFU%jRLz{xadcF zfJ%nuW^es`W~;qpB?`7OcqOAU zCp0(JDvaSqezXU1lYw#To8=;Fhge`V3O2@Y7?DLk+B0Kdr28&9)ksF8z{X1Ko8_Ew z7ssP+0N8_v-Y z4z7^lcPx{2m5fG>dMo|tdu0j(uju-46VK500R>yRSsL36 zqZr9HHO6Xh6Hms4tt=>wZH7^dWSbgeeYlAy^@ z2N^~&l5J{?_2DL-j0?MWjDrlL7|Av@#%ga9PsSbf>i4zV47L3%T3d`)bbYvqC*#7_ z=#FKGhBcr(5OJkd16eHQD#`tN} z#FKFc``8)>8HG}$W{lO|CZ3E78$ItEz&67uMzYO*jMd&Io{S3{GuZd5ZH7^dWSbge zeYlAyQJkO z^-6ZGk|_hDSkG!g@gB5K?Zd4?+-9IvJfkYlryAK0qtvG}XYG`sxI3cM!n(;g(&1Jm z19QU`T`}iIgyv{jn5YL#ZQ?60m>$ErVnB~O z+|{ir6!knJY}KT42{Q!PCiS9WLAK}MkzsTt$xoSuwJ8?Vmu zZH7^dWSjjMZyD*yxU?~YwXYmz7{y4o$(U5rZBNFftzDw>e4AkuHEdJk+p~^vdonI< z?JFc4U>G%w2=}YuuUb7W>me)FjrzMNW`tgyINJ=hoi*s(+G33N_Vi?2+8W*38MYZl zF_LZee0-WnPsXK<7uQv3Y%`2vB-_*&pNi6xacLu?xY|o&n_(0q*`~($beEotEBn|Q z2N{J@q-Kmysp-kMwDGDvn627o7{y4o$@pN_Z*Nb=rHvV^v*a+tC~DXyV^W8{JsFoa zR%L%OvCS}w8n&r1-V@W4acK`H(U4Kw45Nmz;^!#Cf7_>;8KF13GccBG6RCxD*OJEG zc|P7UQeBL?tJ5B9a^yXc9<9;+z#evI+OCnqo=i2eRjzDAhIWaSe%M1)GF-E0sgVq$ z$kxt)-p^ZQ4^hc*&7#8$qsYdb^cgknxjjTB!!?VR8p$wPIu6ry4@bu^Jghsa4$JRx~`fM7atSZD-nYKJ1w@ zh#cl;-l}96C5;-9RgE62WVpW5VTMuE@JKHi-ergyYVRB!(H?d?;^+OaXU8Ahq+Jg-(}Fn73Q=ww*YNQP1BxhfUT*OhjL z>Mh2F)!55c-mGZY!??*%24BH2ifr$8%lXjmQ!P;zxmA6Ko37pLM>3R;>#eF-Y>83X zr)8@$i?pt)Ra$TBGg!ICxXI8uUTKwMkzo{V<;@Dio;gGLcm=~KvOVg{p4&rIGPGl? zXe7fZvOPCd!R(Y3%v6u;T-C*DA9SYTTmR>(OtT*ttdgc$lkH(#GiYn1gQ2==6NXXL za7C9vBkIGoC*#uA+(=`KVH7o7(PhwJojcWjjkxtFp1G z@3kl6()OM|v?}WLO&CT|W32T;4c1{*-NE)`T-us*^<#@+6g9?LFVs-2Vpj2JPsXLK z43% zg9aJi6CEp2)EH~MP=l*bwI4BEPZ-&d6*Pr1^)w>ws%s4>)fISPhqO|~Z)p@*~9 ztzsT#tVBgWc4N<3k2O8XpskS(2KSRW!zjM5F%&~HSPXkIE^V!tD;P#mL+eTmu0+;j zO;5(9t(6}P?(cJkQG8$dAqH~;I;ZK$xU`kQS1^oXBxN&x@5UKqZeYAh!=8*wTlFgJLcNl!9p ziA}X8J656?NvDJus#RPX_9TNgDm|Q|N&*q@nXi=p#_we&hJZC4N(R*7mAFBnEqL#G5Zm>;TD zTnu|ME^XH}8Cs=U#S4Z})X*uRJXfva$|!p>E^SwI8Cs=U#S4Z})X@2%o#F1GRHS>@%%uo$xGELK1ol;4LzKLIzL#ec)?IVSnJ;X*bNNU z@n(*sC*#uANQ1%E&e3I#%o#@UeT}*ps#RPJdonI$ zX=^6}gY&^U-prAVl_uM7Yw5`hWwOD4Am+w26r)fGA?bb7~V_3;3{DqZ`Hc@-JlpoQG+`x z*L9yCtW{hZ_GDbzx;ZI7RI7NwFp3(wUx}ew#g$R^WL(NhVoAt_7Ihf{zlzGBN;}K?Yq{pRrU~-jQ&R5LL(VQk*z&=r62YXm5lXo z>SZk2M$z_NYuOKb=8W}kPG&6HM$y*ExnFA7GiTtNXk7zhI)lkDifo7)7@4TFbGrhp1!>RS{H2c#_i8`(Ug z)GF2yK2L`9urnND7)7?maD`z{^n+D_J#JSpG`hvmT3BJ&lW}Ql{;yydMTW9qg<(&| zrLC2ZQ*Az0$uNoxW%CNdo{URd8GHr9C^EFitT61!xU{veT){Ak4DFRGj6`Wq^c|(D z>@LU3u^2ZQ+O=0Sl3^6th<%?kVVdXm5S0wrXReHQkWnc0Tz9znoTF9ckw!Dnr@brV zO*N8X6xo<3ekQZhb9;zN21dOr<4rY^VHDX|Z@R32e8@YiTng8q0^w3U*zVD9TLC#y{Skg<(&|rLC3Eoo;qRPKHrrC^uIa_GDbz%Ev1hMv#eZAPP@(Ka+DRnpp%acN^6_q~0a(I{xN4UI{)xAtUQ+E~Y3gKnGA zC}^||jY-wI_GDbzSjS!QWSh|_XtWKDN%g(m{e(QPsT01xFK&dGzX>8Hd-~QHtqTFXA5=nriNBYwQ8HuDD0c>jcrDw zpwTuoCe_&6lW}QlM=gzQMx&t7HZ;D>dit(I-=2(1TX(I}*k&{e8f}*bPMn){`wLIT zmCZYxG!8HZ)DWo|U(CM?K=txuT-xY4&-2pQW;ALTt7c68T0zIkxU?~YyqA>5HltC{ zXd7cS`Kt@<$+)z!5_!KWjcrDwpwTuoCVxGmJsFoaGK%-a(%5D+3L0%gZ?aBKvV)9{1yr7cc}9(Y>2IPAMx$VNYZv{H##yv#PrF;YV6R6 zTTutt<7M^Rg)UAW?xq?Y3vnAY^n94jEsa`z(qL6!uice( zm1{Q{8ikYPDoIA8pwTOT(JIYawM27XwpPzdtJ;IO$eatO+GeQjtg`9W7GtdTHt}R! z*c#m(TeZzFiji!SF;;tTqaXLZ{l_n#_4R-I;^^pW-@`Zvp{lBF@1Ar0f#cV|>ds^MXN>d& zI~lv9qr2aIdim;)ek*42_q-<=Veh_h_iK+|cl~|RNJe@#7|{6mJ-=}AT|e}CXgqQ= z8Da0pK|&Q_x}JiK5}a^!ruMJ<Yf_E?=w z`cVwEy*$BA#yB77%h9uv5w_=C8k&z{Xb#G=!GMNV2^jz4S~9}++LcB!w3f=V!GMNV zJ2W1=Eg4}u2c?k=AGLgVSq!D0m4JA*Wmq1-;ifQI%f%-~0ECL`<}=DPQ> LDI+}_3~2mc!n>=> literal 0 HcmV?d00001 diff --git a/src/wroboarm_22/package.xml b/src/wroboarm_22/package.xml new file mode 100644 index 00000000..2ca5eaf6 --- /dev/null +++ b/src/wroboarm_22/package.xml @@ -0,0 +1,36 @@ + + + wroboarm_22 + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the wroboarm_22 with the MoveIt Motion Planning Framework + + Nicholas Underwood + Nicholas Underwood + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + diff --git a/src/wroboarm_22/urdf/wroboarm_22.csv b/src/wroboarm_22/urdf/wroboarm_22.csv new file mode 100644 index 00000000..7b0451ef --- /dev/null +++ b/src/wroboarm_22/urdf/wroboarm_22.csv @@ -0,0 +1,8 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,1.03582841131364E-18,0.015606490089144,1.93574980271696E-18,0,0,0,0.93972387307473,0.00251429022255044,8.65861551705636E-21,-7.37485241366082E-20,0.0044229329573873,2.27711298016934E-20,0.0021336882069818,0,0,0,0,0,0,package://wroboarm_22/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/base_link.STL,,base-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +shoulderBase_link,-0.0122586821821901,0.128577249577976,-0.00370544350042927,0,0,0,0.996366805002727,0.00652370094208947,0.0010646616036529,0.000283127005277364,0.0050231810993567,6.82113719161566E-05,0.0086334538259584,0,0,0,0,0,0,package://wroboarm_22/meshes/shoulderBase_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/shoulderBase_link.STL,,Shoulder-1,Origin_turntable,Axis_turntable,turntable,continuous,0,0,0.035865,1.5708,0,-0.25129,base_link,0,1,0,,,,,,,,,,,, +upperArm_link,0.0143973999054565,-3.20061625687929E-05,0.279348384794928,0,0,0,0.985210343915811,0.0237982224738339,-8.87977733299251E-07,-0.00388532856230119,0.0293594722597602,-7.12944369971275E-06,0.00625034839320104,0,0,0,0,0,0,package://wroboarm_22/meshes/upperArm_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/upperArm_link.STL,,UpperArm-1,Origin_shoulder,Axis_shoulder,shoulder,continuous,0,0.18098,0.043071,-1.1021,0,0,shoulderBase_link,1,0,0,,,,,,,,,,,, +forarm_link,1.84574577843932E-15,0.0550154986611603,0.120152803034247,0,0,0,0.517796101730459,0.00404850874710771,2.11419423634673E-18,-1.54871504075976E-17,0.00375548329392274,-0.000377385363706529,0.000788327838226562,0,0,0,0,0,0,package://wroboarm_22/meshes/forarm_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/forarm_link.STL,,Forarm-1,Origin_elbow,Axis_elbow,elbow,continuous,0,0,0.46634,2.4936,0,0,upperArm_link,1,0,0,,,,,,,,,,,, +wirstBase_link,-1.94289029309402E-16,-1.66533453693773E-15,0.0987034845564234,0,0,0,0.301763082175337,0.00210344289702837,-2.71050543121376E-19,-2.24971950790742E-18,0.00305016250706558,1.85669622038143E-18,0.00107609106501786,0,0,0,0,0,0,package://wroboarm_22/meshes/wirstBase_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/wirstBase_link.STL,,WristStructure-1,Origin_forearm_roll,Axis_forearm_roll,forearm_roll,continuous,0,0.04445,0.2285,0,0,0,forarm_link,0,0,1,,,,,,,,,,,, +wrist_link,4.30211422042248E-16,-2.77555756156289E-17,0,0,0,0,0.447707117943176,0.000367702266904795,1.79994501291539E-19,-7.45388993583784E-19,0.000843427086265753,-1.35525271560688E-20,0.00084474612356066,0,0,0,0,0,0,package://wroboarm_22/meshes/wrist_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/wrist_link.STL,,Wrist-1,Origin_wrist_pitch,Axis_wrist_pitch,wrist_pitch,continuous,0,0,0.23716,-1.333,0,0,wirstBase_link,1,0,0,,,,,,,,,,,, +manipulator_link,-0.000730323740460503,-0.000899365669252,0.0490560099953423,0,0,0,0.397420225554811,0.00159684305263878,-3.07243227891656E-06,2.53235579936151E-05,0.0016343224414688,5.00292290874635E-05,0.000256322563994287,0,0,0,0,0,0,package://wroboarm_22/meshes/manipulator_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_22/meshes/manipulator_link.STL,,Manipulator-1,Origin_wrist_roll,Axis_wrist_roll,wrist_roll,continuous,0,0,0,0,0,0.096862,wrist_link,0,0,1,,,,,,,,,,,, diff --git a/src/wroboarm_22/urdf/wroboarm_22.urdf b/src/wroboarm_22/urdf/wroboarm_22.urdf new file mode 100644 index 00000000..8802c66c --- /dev/null +++ b/src/wroboarm_22/urdf/wroboarm_22.urdf @@ -0,0 +1,281 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + / + + + \ No newline at end of file diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh index 422617d2..8d1bda56 100755 --- a/test_arm_control_stack.sh +++ b/test_arm_control_stack.sh @@ -2,7 +2,7 @@ ROOT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" cd $ROOT_DIR ./assemble.py build -gnome-terminal -- roscore +# gnome-terminal -- roscore rostopic list &>./out.temp; COUNT=$(grep -Ec "Unable to communicate with master" out.temp) while [ $COUNT -ne 0 ]; do @@ -11,11 +11,6 @@ while [ $COUNT -ne 0 ]; do done rm ./out.temp; export WROVER_LOCAL=true +export WROVER_HW=MOCK gnome-terminal -- roslaunch wr_entry_point test_arm.launch -gnome-terminal -- roslaunch wr_control_drive_arm std.launch -gnome-terminal -- roslaunch wroboarm_21 demo_test.launch -#gnome-terminal -- rqt_plot /control/arm/00/setpoint /control/arm/00/feedback -sleep 10 -gnome-terminal -- /bin/sh -c "rosrun wr_logic_teleop_arm ArmTeleopLogic; while true; do sleep 1; done" -gnome-terminal -- roslaunch wreadinput test_xbox360.launch -cd - +