Compare commits

..

No commits in common. "056fa3de682697648b3ebab3176a940330440458" and "8527b2411e6c3c8f4a53b29983482114dc9a7e85" have entirely different histories.

6 changed files with 184 additions and 198 deletions

View file

@ -1,8 +1,7 @@
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
controller_names:
- joint_trajectory_controller
joint_trajectory_controller:
joint_trajectory_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true

View file

@ -1,10 +1,4 @@
move_group:
planning_plugin: ompl_interface/OMPLPlanner
request_adapters:
default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints
start_state_max_bounds_error: 0.1
planner_configs:
planner_configs:
SBLkConfigDefault:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@ -127,7 +121,7 @@ move_group:
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
ar3_arm:
ar3_arm:
default_planner_config: RRTConnectkConfigDefault
planner_configs:
- SBLkConfigDefault
@ -153,4 +147,3 @@ move_group:
- LazyPRMstarkConfigDefault
- SPARSkConfigDefault
- SPARStwokConfigDefault

View file

@ -1,4 +0,0 @@
publish_planning_scene: true
publish_geometry_updates: true
publish_state_updates: true
publish_transforms_updates: true

View file

@ -1,4 +0,0 @@
moveit_manage_controllers: true
trajectory_execution.allowed_execution_duration_scaling: 1.2
trajectory_execution.allowed_goal_duration_margin: 0.5
trajectory_execution.allowed_start_tolerance: 0.1

View file

@ -1,7 +1,8 @@
from os.path import join
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.actions import ExecuteProcess
import yaml
@ -30,7 +31,7 @@ def generate_launch_description():
executable='rviz2',
output='log',
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
parameters=[robot_description, robot_description_semantic, robot_description_kinematics],
),
# Attatch robot to map
@ -38,7 +39,7 @@ def generate_launch_description():
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link'],
),
Node(
@ -73,30 +74,25 @@ def generate_launch_description():
)
],
output="screen",
)
),
# Controller
Node(
package='controller_manager',
executable='ros2_control_node',
output='screen',
parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
parameters=[robot_description, join(config_root, 'controller_manager.yaml')],
),
# Start controllers
ExecuteProcess(
cmd=['ros2 run controller_manager spawner.py joint_trajectory_controller'],
shell=True,
output='screen'),
output='screen',
),
ExecuteProcess(
cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
shell=True,
output='screen'),
# Run platform
Node(
package='cobot_platform',
executable='move',
output='screen',
),
@ -109,11 +105,9 @@ def generate_launch_description():
robot_description,
robot_description_semantic,
robot_description_kinematics,
get_yaml(join(config_root, 'ompl_planning.yaml')),
get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")),
],
{'move_group': join(config_root, 'ompl_planning.yaml')},
{'moveit_simple_controller_manager': join(config_root, "ar3_controllers.yaml")},
]
),
# Robot state publisher
@ -121,5 +115,14 @@ def generate_launch_description():
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]),
parameters=[robot_description],
),
# Interface with arduino
Node(
package='cobot_platform',
executable='move',
output='screen',
),
])

View file

@ -46,11 +46,11 @@ class CobotMove(Node):
print(self._serial.readline())
def _move_j1_cb(self, msg):
self._cur[0] = int(msg.data / pi / 2. * 8000)
self._cur[0] = int(msg.data / pi / 2. * 2000)
self._move_servo()
def _move_j2_cb(self, msg):
self._cur[1] = int(msg.data / pi / 2. * 10000)
self._cur[1] = int(msg.data / pi / 2. * 3840.64)
self._move_servo()
def _move_j3_cb(self, msg):
@ -105,4 +105,3 @@ def main(args=None):
if __name__ == '__main__':
main()