Skip to content

Commit

Permalink
Adapt simulation launch files and add prefix support for simulation a…
Browse files Browse the repository at this point in the history
…nd MoveIt (#100)

* Created interface class and put the RPC handlers in robot folder

* Now the robot functions interface work just like before

* Added templates and generation for simulated services

* Added the simulation services to the driver headers

* Added newly generated files to repo

* Added simulation option to the driver so the gazebo package can launch the kortex_driver too, in a different mode

* Adapt joint_limits files to correctly take prefix option

* Change description and control files to support prefix option

* Change MoveIt config launch and config files to enable prefix option - only SRDF modifications remain

* Added xacro usage in SRDF files to be able to prefix joint names

* Adjust ompl planning launch and config files to support prefixing joint names

* Remove --inorder from xacro calls since it's default now
  • Loading branch information
alexvannobel authored Jun 16, 2020
1 parent 73da179 commit 7d2ba77
Show file tree
Hide file tree
Showing 99 changed files with 1,869 additions and 1,714 deletions.
Original file line number Diff line number Diff line change
@@ -1,73 +1,73 @@
# Publish all joint states -----------------------------------
joint_state_controller:
$(arg prefix)joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

gen3_joint_trajectory_controller:
$(arg prefix)gen3_joint_trajectory_controller:
type: effort_controllers/JointTrajectoryController
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- $(arg prefix)joint_1
- $(arg prefix)joint_2
- $(arg prefix)joint_3
- $(arg prefix)joint_4
- $(arg prefix)joint_5
- $(arg prefix)joint_6
constraints:
goal_time: 1.0
stopped_velocity_tolerance: 0.5
stop_trajectory_duration: 1.0
state_publish_rate: 25
action_monitor_rate: 25
gains:
joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0}
joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1}
$(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
$(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0}
$(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1}

joint_1_position_controller:
joint: joint_1
$(arg prefix)joint_1_position_controller:
joint: $(arg prefix)joint_1
pid:
p: 3000.0
i: 0.0
d: 2.0
type: effort_controllers/JointPositionController

joint_2_position_controller:
joint: joint_2
$(arg prefix)joint_2_position_controller:
joint: $(arg prefix)joint_2
pid:
p: 50000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_3_position_controller:
joint: joint_3
$(arg prefix)joint_3_position_controller:
joint: $(arg prefix)joint_3
pid:
p: 50000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_4_position_controller:
joint: joint_4
$(arg prefix)joint_4_position_controller:
joint: $(arg prefix)joint_4
pid:
p: 750.0
i: 0.0
d: 0.2
type: effort_controllers/JointPositionController

joint_5_position_controller:
joint: joint_5
$(arg prefix)joint_5_position_controller:
joint: $(arg prefix)joint_5
pid:
p: 5000.0
i: 0.0
d: 1.0
type: effort_controllers/JointPositionController

joint_6_position_controller:
joint: joint_6
$(arg prefix)joint_6_position_controller:
joint: $(arg prefix)joint_6
pid:
p: 100.0
i: 0.0
Expand Down
Original file line number Diff line number Diff line change
@@ -1,83 +1,83 @@
# Publish all joint states -----------------------------------
joint_state_controller:
$(arg prefix)joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

gen3_joint_trajectory_controller:
$(arg prefix)gen3_joint_trajectory_controller:
type: effort_controllers/JointTrajectoryController
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- joint_7
- $(arg prefix)joint_1
- $(arg prefix)joint_2
- $(arg prefix)joint_3
- $(arg prefix)joint_4
- $(arg prefix)joint_5
- $(arg prefix)joint_6
- $(arg prefix)joint_7
constraints:
goal_time: 1.0
stopped_velocity_tolerance: 0.5
stop_trajectory_duration: 1.0
state_publish_rate: 25
action_monitor_rate: 25
gains:
joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0}
joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1}
$(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
$(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0}
$(arg prefix)joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1}

joint_1_position_controller:
joint: joint_1
$(arg prefix)joint_1_position_controller:
joint: $(arg prefix)joint_1
pid:
p: 3000.0
i: 0.0
d: 2.0
type: effort_controllers/JointPositionController

joint_2_position_controller:
joint: joint_2
$(arg prefix)joint_2_position_controller:
joint: $(arg prefix)joint_2
pid:
p: 50000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_3_position_controller:
joint: joint_3
$(arg prefix)joint_3_position_controller:
joint: $(arg prefix)joint_3
pid:
p: 3000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_4_position_controller:
joint: joint_4
$(arg prefix)joint_4_position_controller:
joint: $(arg prefix)joint_4
pid:
p: 50000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_5_position_controller:
joint: joint_5
$(arg prefix)joint_5_position_controller:
joint: $(arg prefix)joint_5
pid:
p: 750.0
i: 0.0
d: 0.2
type: effort_controllers/JointPositionController

joint_6_position_controller:
joint: joint_6
$(arg prefix)joint_6_position_controller:
joint: $(arg prefix)joint_6
pid:
p: 5000.0
i: 0.0
d: 1.0
type: effort_controllers/JointPositionController

joint_7_position_controller:
joint: joint_7
$(arg prefix)joint_7_position_controller:
joint: $(arg prefix)joint_7
pid:
p: 100.0
i: 0.0
Expand Down
Original file line number Diff line number Diff line change
@@ -1,73 +1,73 @@
# Publish all joint states -----------------------------------
joint_state_controller:
$(arg prefix)joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

gen3_lite_joint_trajectory_controller:
$(arg prefix)gen3_lite_joint_trajectory_controller:
type: effort_controllers/JointTrajectoryController
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- $(arg prefix)joint_1
- $(arg prefix)joint_2
- $(arg prefix)joint_3
- $(arg prefix)joint_4
- $(arg prefix)joint_5
- $(arg prefix)joint_6
constraints:
goal_time: 1.0
stopped_velocity_tolerance: 0.5
stop_trajectory_duration: 1.0
state_publish_rate: 25
action_monitor_rate: 25
gains:
joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0}
joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1}
$(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
$(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0}
$(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
$(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1}

joint_1_position_controller:
joint: joint_1
$(arg prefix)joint_1_position_controller:
joint: $(arg prefix)joint_1
pid:
p: 3000.0
i: 0.0
d: 2.0
type: effort_controllers/JointPositionController

joint_2_position_controller:
joint: joint_2
$(arg prefix)joint_2_position_controller:
joint: $(arg prefix)joint_2
pid:
p: 50000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_3_position_controller:
joint: joint_3
$(arg prefix)joint_3_position_controller:
joint: $(arg prefix)joint_3
pid:
p: 50000.0
i: 0.0
d: 0.0
type: effort_controllers/JointPositionController

joint_4_position_controller:
joint: joint_4
$(arg prefix)joint_4_position_controller:
joint: $(arg prefix)joint_4
pid:
p: 750.0
i: 0.0
d: 0.2
type: effort_controllers/JointPositionController

joint_5_position_controller:
joint: joint_5
$(arg prefix)joint_5_position_controller:
joint: $(arg prefix)joint_5
pid:
p: 5000.0
i: 0.0
d: 1.0
type: effort_controllers/JointPositionController

joint_6_position_controller:
joint: joint_6
$(arg prefix)joint_6_position_controller:
joint: $(arg prefix)joint_6
pid:
p: 100.0
i: 0.0
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
gen3_lite_2f_gripper_controller:
$(arg prefix)gen3_lite_2f_gripper_controller:
type: position_controllers/GripperActionController
joint: right_finger_bottom_joint
joint: $(arg prefix)right_finger_bottom_joint
action_monitor_rate: 100

gazebo_ros_control:
pid_gains:
right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0}
right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0}
left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0}
left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0}
$(arg prefix)right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0}
$(arg prefix)right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0}
$(arg prefix)left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0}
$(arg prefix)left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0}
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
robotiq_2f_140_gripper_controller:
$(arg prefix)robotiq_2f_140_gripper_controller:
type: position_controllers/GripperActionController
joint: finger_joint
joint: $(arg prefix)finger_joint
action_monitor_rate: 100

gazebo_ros_control:
pid_gains:
finger_joint: {p: 10.0, i: 0.0, d: 0.01}
right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00}
right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
$(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01}
$(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00}
$(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
$(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
$(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
$(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
robotiq_2f_85_gripper_controller:
$(arg prefix)robotiq_2f_85_gripper_controller:
type: position_controllers/GripperActionController
joint: finger_joint
joint: $(arg prefix)finger_joint
action_monitor_rate: 100

gazebo_ros_control:
pid_gains:
finger_joint: {p: 10.0, i: 0.0, d: 0.01}
right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00}
right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
$(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01}
$(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00}
$(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
$(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01}
$(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
$(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001}
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
initial_joint_positions: "-J joint_1 1.57
-J joint_2 -0.35
-J joint_3 -2.00
-J joint_4 0
-J joint_5 -1.00
-J joint_6 1.57 "
initial_joint_positions: "-J $(arg prefix)joint_1 1.57
-J $(arg prefix)joint_2 -0.35
-J $(arg prefix)joint_3 -2.00
-J $(arg prefix)joint_4 0
-J $(arg prefix)joint_5 -1.00
-J $(arg prefix)joint_6 1.57 "
Loading

0 comments on commit 7d2ba77

Please sign in to comment.