Skip to content

Commit 2f3f056

Browse files
MarqRazzMarqRazz
authored
Fix mock_hardware and enable simulating gen3_lite (Kinovarobotics#196)
* Add additional guards if user sets use_fake_hardware and use_internal_bus_gripper_comm true
1 parent 0e899f0 commit 2f3f056

File tree

11 files changed

+194
-21
lines changed

11 files changed

+194
-21
lines changed

‎README.md

+11
Original file line numberDiff line numberDiff line change
@@ -199,6 +199,17 @@ ros2 launch kinova_gen3_6dof_robotiq_2f_85_moveit_config robot.launch.py \
199199
use_fake_hardware:=true
200200
```
201201

202+
Alternatively, if you wish to use the Kinova Gen3_lite's 6 DoF variant:
203+
204+
```bash
205+
ros2 launch kortex_bringup gen3.launch.py \
206+
robot_ip:=yyy.yyy.yyy.yyy \
207+
use_fake_hardware:=true \
208+
robot_type:=gen3_lite \
209+
gripper:=gen3_lite_2f \
210+
dof:=6
211+
```
212+
202213
To simulate the 7dof Kinova Gen3 robot with ignition run the following:
203214

204215
```bash

‎kortex_bringup/launch/gen3.launch.py

+12-2
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,14 @@
2323
def generate_launch_description():
2424
# Declare arguments
2525
declared_arguments = []
26+
declared_arguments.append(
27+
DeclareLaunchArgument(
28+
"robot_type",
29+
default_value="gen3",
30+
description="Type/series of robot.",
31+
choices=["gen3", "gen3_lite"],
32+
)
33+
)
2634
declared_arguments.append(
2735
DeclareLaunchArgument(
2836
"robot_ip",
@@ -64,8 +72,9 @@ def generate_launch_description():
6472
declared_arguments.append(
6573
DeclareLaunchArgument(
6674
"gripper",
67-
default_value='"robotiq_2f_85"',
75+
default_value="robotiq_2f_85",
6876
description="Name of the gripper attached to the arm",
77+
choices=["robotiq_2f_85", "gen3_lite_2f"],
6978
)
7079
)
7180
declared_arguments.append(
@@ -101,6 +110,7 @@ def generate_launch_description():
101110
)
102111

103112
# Initialize Arguments
113+
robot_type = LaunchConfiguration("robot_type")
104114
robot_ip = LaunchConfiguration("robot_ip")
105115
dof = LaunchConfiguration("dof")
106116
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
@@ -117,7 +127,7 @@ def generate_launch_description():
117127
base_launch = IncludeLaunchDescription(
118128
PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/kortex_control.launch.py"]),
119129
launch_arguments={
120-
"robot_type": "gen3",
130+
"robot_type": robot_type,
121131
"robot_ip": robot_ip,
122132
"dof": dof,
123133
"use_fake_hardware": use_fake_hardware,

‎kortex_bringup/launch/kortex_control.launch.py

+7-2
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,10 @@ def launch_setup(context, *args, **kwargs):
5656
use_internal_bus_gripper_comm = LaunchConfiguration("use_internal_bus_gripper_comm")
5757
gripper_joint_name = LaunchConfiguration("gripper_joint_name")
5858

59+
# if we are using fake hardware then we can't use the internal gripper communications of the hardware
60+
if use_fake_hardware.parse:
61+
use_internal_bus_gripper_comm = "false"
62+
5963
robot_description_content = Command(
6064
[
6165
PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -107,7 +111,7 @@ def launch_setup(context, *args, **kwargs):
107111
robot_controllers = PathJoinSubstitution(
108112
[
109113
FindPackageShare(description_package),
110-
"arms/gen3/" + dof.perform(context) + "dof/config",
114+
"arms/" + robot_type.perform(context) + "/" + dof.perform(context) + "dof/config",
111115
controllers_file,
112116
]
113117
)
@@ -174,13 +178,14 @@ def launch_setup(context, *args, **kwargs):
174178
package="controller_manager",
175179
executable="spawner",
176180
arguments=[robot_hand_controller, "-c", "/controller_manager"],
177-
condition=IfCondition(use_internal_bus_gripper_comm),
178181
)
179182

183+
# only start the fault controller if we are using hardware
180184
fault_controller_spawner = Node(
181185
package="controller_manager",
182186
executable="spawner",
183187
arguments=[fault_controller, "-c", "/controller_manager"],
188+
condition=IfCondition(use_internal_bus_gripper_comm),
184189
)
185190

186191
nodes_to_start = [

‎kortex_description/arms/gen3/7dof/urdf/kortex.ros2_control.xacro

+9-7
Original file line numberDiff line numberDiff line change
@@ -136,13 +136,15 @@
136136
<state_interface name="velocity"/>
137137
<state_interface name="effort"/>
138138
</joint>
139-
<xacro:if value="${use_internal_bus_gripper_comm}">
140-
<joint name="${prefix}${gripper_joint_name}">
141-
<command_interface name="position" />
142-
<state_interface name="position"/>
143-
<state_interface name="velocity"/>
144-
</joint>
145-
</xacro:if>
139+
<xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
140+
<xacro:if value="${use_internal_bus_gripper_comm}">
141+
<joint name="${prefix}${gripper_joint_name}">
142+
<command_interface name="position" />
143+
<state_interface name="position"/>
144+
<state_interface name="velocity"/>
145+
</joint>
146+
</xacro:if>
147+
</xacro:unless>
146148
</ros2_control>
147149
</xacro:macro>
148150

‎kortex_description/arms/gen3_lite/6dof/config/ros2_controllers.yaml

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
controller_manager:
2+
ros__parameters:
3+
update_rate: 1000 # Hz
4+
5+
joint_state_broadcaster:
6+
type: joint_state_broadcaster/JointStateBroadcaster
7+
8+
joint_trajectory_controller:
9+
type: joint_trajectory_controller/JointTrajectoryController
10+
11+
twist_controller:
12+
type: picknik_twist_controller/PicknikTwistController
13+
14+
robotiq_gripper_controller:
15+
type: position_controllers/GripperActionController
16+
17+
fault_controller:
18+
type: picknik_reset_fault_controller/PicknikResetFaultController
19+
20+
joint_trajectory_controller:
21+
ros__parameters:
22+
joints:
23+
- joint_1
24+
- joint_2
25+
- joint_3
26+
- joint_4
27+
- joint_5
28+
- joint_6
29+
command_interfaces:
30+
- position
31+
state_interfaces:
32+
- position
33+
- velocity
34+
state_publish_rate: 100.0
35+
action_monitor_rate: 20.0
36+
allow_partial_joints_goal: false
37+
constraints:
38+
stopped_velocity_tolerance: 0.0
39+
goal_time: 0.0
40+
41+
twist_controller:
42+
ros__parameters:
43+
joint: tcp
44+
interface_names:
45+
- twist.linear.x
46+
- twist.linear.y
47+
- twist.linear.z
48+
- twist.angular.x
49+
- twist.angular.y
50+
- twist.angular.z
51+
52+
robotiq_gripper_controller:
53+
ros__parameters:
54+
default: true
55+
joint: right_finger_bottom_joint
56+
allow_stalling: true

‎kortex_description/arms/gen3_lite/6dof/urdf/kortex.ros2_control.xacro

+9-7
Original file line numberDiff line numberDiff line change
@@ -125,13 +125,15 @@
125125
<state_interface name="velocity"/>
126126
<state_interface name="effort"/>
127127
</joint>
128-
<xacro:if value="${use_internal_bus_gripper_comm}">
129-
<joint name="${prefix}right_finger_bottom_joint">
130-
<command_interface name="position"/>
131-
<state_interface name="position"/>
132-
<state_interface name="velocity"/>
133-
</joint>
134-
</xacro:if>
128+
<xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
129+
<xacro:if value="${use_internal_bus_gripper_comm}">
130+
<joint name="${prefix}right_finger_bottom_joint">
131+
<command_interface name="position"/>
132+
<state_interface name="position"/>
133+
<state_interface name="velocity"/>
134+
</joint>
135+
</xacro:if>
136+
</xacro:unless>
135137
</ros2_control>
136138
</xacro:macro>
137139

‎kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f.ros2_control.xacro

+74
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
4+
<xacro:macro name="gripper_ros2_control" params="
5+
prefix
6+
use_fake_hardware:=false
7+
fake_sensor_commands:=false
8+
sim_ignition:=false
9+
sim_isaac:=false
10+
isaac_joint_commands:=/isaac_joint_commands
11+
isaac_joint_states:=/isaac_joint_states
12+
use_internal_bus_gripper_comm:=false">
13+
14+
<ros2_control name="GripperHardwareInterface" type="system">
15+
<hardware>
16+
<xacro:if value="${sim_isaac}">
17+
<plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
18+
<param name="joint_commands_topic">${isaac_joint_commands}</param>
19+
<param name="joint_states_topic">${isaac_joint_states}</param>
20+
<param name="trigger_joint_command_threshold">0.02</param>
21+
</xacro:if>
22+
<xacro:if value="${sim_ignition}">
23+
<plugin>ign_ros2_control/IgnitionSystem</plugin>
24+
</xacro:if>
25+
<xacro:if value="${use_fake_hardware}">
26+
<plugin>mock_components/GenericSystem</plugin>
27+
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
28+
<param name="state_following_offset">0.0</param>
29+
</xacro:if>
30+
</hardware>
31+
32+
<!-- Joint interfaces -->
33+
<!-- With Ignition or Hardware, they handle mimic joints, so we only need this command interface activated -->
34+
<joint name="${prefix}right_finger_bottom_joint">
35+
<command_interface name="position" />
36+
<state_interface name="position">
37+
<param name="initial_value">0.85</param>
38+
</state_interface>
39+
<state_interface name="velocity"/>
40+
</joint>
41+
<!-- When simulating we need to include the rest of the gripper joints -->
42+
<xacro:if value="${use_fake_hardware or sim_isaac or sim_ignition}">
43+
<joint name="${prefix}right_finger_tip_joint">
44+
<param name="mimic">${prefix}right_finger_bottom_joint</param>
45+
<param name="multiplier">-1</param>
46+
<xacro:unless value="${sim_ignition}">
47+
<command_interface name="position"/>
48+
<state_interface name="position"/>
49+
<state_interface name="velocity"/>
50+
</xacro:unless>
51+
</joint>
52+
<joint name="${prefix}left_finger_bottom_joint">
53+
<param name="mimic">${prefix}right_finger_bottom_joint</param>
54+
<param name="multiplier">1</param>
55+
<xacro:unless value="${sim_ignition}">
56+
<command_interface name="position"/>
57+
<state_interface name="position"/>
58+
<state_interface name="velocity"/>
59+
</xacro:unless>
60+
</joint>
61+
<joint name="${prefix}left_finger_tip_joint">
62+
<param name="mimic">${prefix}right_finger_bottom_joint</param>
63+
<param name="multiplier">-1</param>
64+
<xacro:unless value="${sim_ignition}">
65+
<command_interface name="position"/>
66+
<state_interface name="position"/>
67+
<state_interface name="velocity"/>
68+
</xacro:unless>
69+
</joint>
70+
</xacro:if>
71+
</ros2_control>
72+
</xacro:macro>
73+
74+
</robot>

‎kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro

+11
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,17 @@
1212
isaac_joint_states:=/isaac_joint_states
1313
use_internal_bus_gripper_comm:=true">
1414

15+
<xacro:include filename="$(find kortex_description)/grippers/gen3_lite_2f/urdf/gen3_lite_2f.ros2_control.xacro" />
16+
<xacro:gripper_ros2_control
17+
prefix="${prefix}"
18+
use_fake_hardware="${use_fake_hardware}"
19+
fake_sensor_commands="${fake_sensor_commands}"
20+
sim_ignition="${sim_ignition}"
21+
sim_isaac="${sim_isaac}"
22+
isaac_joint_commands="${isaac_joint_commands}"
23+
isaac_joint_states="${isaac_joint_states}"
24+
use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"/>
25+
1526
<!-- Tool frame used by the arm -->
1627
<link name="${prefix}tool_frame"/>
1728
<joint name="${prefix}tool_frame_joint" type="fixed">

‎kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_macro.xacro

+2-1
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,8 @@
88
sim_ignition:=false
99
sim_isaac:=false
1010
isaac_joint_commands:=/isaac_joint_commands
11-
isaac_joint_states:=/isaac_joint_states">
11+
isaac_joint_states:=/isaac_joint_states
12+
use_internal_bus_gripper_comm:=false">
1213
<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
1314

1415
<!-- Hardware talks directly to the gripper so we don't need ros2_control unless we are simulating -->

‎kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_macro.xacro

+1-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
sim_isaac:=false
1010
isaac_joint_commands:=/isaac_joint_commands
1111
isaac_joint_states:=/isaac_joint_states
12-
use_internal_bus_gripper_comm:=true">
12+
use_internal_bus_gripper_comm:=false">
1313
<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
1414

1515
<!-- Hardware talks directly to the gripper so we don't need ros2_control unless we are simulating -->

‎kortex_description/robots/kortex_robot.xacro

+2-1
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,8 @@
8686
sim_ignition="${sim_ignition}"
8787
sim_isaac="${sim_isaac}"
8888
isaac_joint_commands="${isaac_joint_commands}"
89-
isaac_joint_states="${isaac_joint_states}"/>
89+
isaac_joint_states="${isaac_joint_states}"
90+
use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"/>
9091
</xacro:unless>
9192

9293
</xacro:macro>

0 commit comments

Comments
 (0)