diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml index 2a0f1e9c..2adf7e67 100644 --- a/src/lab_sim/config/config.yaml +++ b/src/lab_sim/config/config.yaml @@ -80,6 +80,7 @@ ros2_control: - "servo_controller" - "joint_trajectory_admittance_controller" - "velocity_force_controller" + - "joint_velocity_controller" # Any controllers here will not be spawned by MoveIt Pro. # [Optional, default=[]] controllers_not_managed: [] diff --git a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml index 64d594b9..e8f08d14 100644 --- a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml @@ -15,6 +15,8 @@ controller_manager: type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController velocity_force_controller: type: velocity_force_controller/VelocityForceController + joint_velocity_controller: + type: joint_velocity_controller/JointVelocityController joint_state_broadcaster: @@ -278,3 +280,33 @@ velocity_force_controller: # command_joints: [] # The type of command interface to use. Supported values are "position" and "velocity". command_interfaces: ["position"] + +joint_velocity_controller: + ros__parameters: + # Joint group to control. + planning_group_name: manipulator + # Maximum joint-space velocities. + max_joint_velocity: + - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 + # Maximum joint-space accelerations. + max_joint_acceleration: + - 2.0 + - 2.0 + - 2.0 + - 2.0 + - 2.0 + - 2.0 + - 2.0 + command_interfaces: ["position"] + # Padding (in radians) to add to joint position limits as a safety margin. + joint_limit_position_tolerance: 0.02 + # Timeout in seconds after which the controller will stop motion if no new commands are received. + command_timeout: 0.2 + # Rate in Hz at which the controller will publish the state. + state_publish_rate: 20 diff --git a/src/lab_sim/config/moveit/joint_jog.yaml b/src/lab_sim/config/moveit/joint_jog.yaml new file mode 100644 index 00000000..6d80c7e9 --- /dev/null +++ b/src/lab_sim/config/moveit/joint_jog.yaml @@ -0,0 +1,4 @@ +# Planning groups to use in JointJog, and their corresponding JVC controllers. +# The number of elements in `planning_groups` and `controllers` must match. +planning_groups: ['manipulator'] +controllers: ['joint_velocity_controller'] diff --git a/src/lab_sim/objectives/joint_jog_example.xml b/src/lab_sim/objectives/joint_jog_example.xml new file mode 100644 index 00000000..b9fe2344 --- /dev/null +++ b/src/lab_sim/objectives/joint_jog_example.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + +