Skip to content

Commit 39d10ca

Browse files
kineticsystemkineticsystem
committed
Gripper implementation for sim.
1 parent ad0c652 commit 39d10ca

File tree

8 files changed

+102
-12
lines changed

8 files changed

+102
-12
lines changed

‎src/moveit_pro_franka_configs/franka_arm_sim/config/config.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@ ros2_control:
8484
# for running the application.
8585
# [Optional, default=[]]
8686
controllers_active_at_startup:
87+
- "franka_gripper_controller"
8788
- "joint_state_broadcaster"
8889
- "joint_trajectory_controller"
8990
# Load but do not start these controllers so they can be activated later if needed.

‎src/moveit_pro_franka_configs/franka_arm_sim/config/control/franka_ros2_control.yaml‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@ controller_manager:
99
type: joint_trajectory_controller/JointTrajectoryController
1010
velocity_force_controller:
1111
type: velocity_force_controller/VelocityForceController
12-
12+
franka_gripper_controller:
13+
type: position_controllers/GripperActionController
1314
joint_trajectory_controller:
1415
ros__parameters:
1516
command_interfaces:
@@ -34,6 +35,14 @@ joint_trajectory_controller:
3435
fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
3536
fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
3637

38+
franka_gripper_controller:
39+
ros__parameters:
40+
default: true
41+
joint: fr3_finger_joint1
42+
allow_stalling: true
43+
stall_timeout: 0.1
44+
goal_tolerance: 0.005
45+
3746
servo_controller:
3847
ros__parameters:
3948
command_interfaces:

‎src/moveit_pro_franka_configs/franka_arm_sim/description/franka.urdf.xacro‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,17 @@
112112
<state_interface name="velocity"/>
113113
<state_interface name="effort"/>
114114
</joint>
115+
<joint name="fr3_finger_joint1">
116+
<command_interface name="position">
117+
<param name="min">-0.0</param>
118+
<param name="max">0.04</param>
119+
</command_interface>
120+
<state_interface name="position">
121+
<param name="initial_value">0.035</param>
122+
</state_interface>
123+
<state_interface name="velocity"/>
124+
<state_interface name="effort"/>
125+
</joint>
115126
<hardware>
116127
<plugin>picknik_mujoco_ros/MujocoSystem</plugin>
117128
<param name="mujoco_model">$(arg mujoco_model)</param>

‎src/moveit_pro_franka_configs/franka_arm_sim/description/mujoco/franka3.xml‎

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -402,15 +402,13 @@
402402
class="fr3"
403403
name="fr3_finger_joint1"
404404
joint="fr3_finger_joint1"
405-
kp="1000"
406-
kv="100"
407-
/>
408-
<position
409-
class="fr3"
410-
name="fr3_finger_joint2"
411-
joint="fr3_finger_joint2"
412-
kp="1000"
413-
kv="100"
405+
kp="5000"
406+
kv="500"
414407
/>
415408
</actuator>
409+
410+
<equality>
411+
<joint joint1="fr3_finger_joint1" joint2="fr3_finger_joint2" polycoef="0 1 0 0 0" />
412+
</equality>
413+
416414
</mujoco>

‎src/moveit_pro_franka_configs/franka_arm_sim/description/mujoco/scene.xml‎

Lines changed: 25 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -98,11 +98,35 @@
9898
</body>
9999
</worldbody>
100100

101+
101102
<keyframe>
103+
<!-- Home position for FR3 robot with gripper
104+
qpos:
105+
fr3_joint1,
106+
fr3_joint2,
107+
fr3_joint3,
108+
fr3_joint4,
109+
fr3_joint5,
110+
fr3_joint6,
111+
fr3_joint7,
112+
fr3_finger1,
113+
fr3_finger2
114+
...
115+
ctrl:
116+
fr3_joint1,
117+
fr3_joint2,
118+
fr3_joint3,
119+
fr3_joint4,
120+
fr3_joint5,
121+
fr3_joint6,
122+
fr3_joint7,
123+
fr3_finger1,
124+
fr3_finger2
125+
-->
102126
<key
103127
name="home"
104128
qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035 0.035 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.06 0.42 0.96 0 0 0 0"
105-
ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035 0.035"
129+
ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035"
106130
/>
107131
</keyframe>
108132
</mujoco>

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/close_gripper.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Close Gripper">
3+
<BehaviorTree ID="Close Gripper" _description="Close the gripper">
4+
<Control ID="Sequence" name="root">
5+
<Action
6+
ID="MoveGripperAction"
7+
gripper_command_action_name="/franka_gripper_controller/gripper_cmd"
8+
position="0"
9+
timeout="10.000000"
10+
/>
11+
</Control>
12+
</BehaviorTree>
13+
<TreeNodesModel>
14+
<SubTree ID="Close Gripper">
15+
<MetadataFields>
16+
<Metadata subcategory="Grasping" />
17+
<Metadata runnable="true" />
18+
</MetadataFields>
19+
</SubTree>
20+
</TreeNodesModel>
21+
</root>

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/open_gripper.xml‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Open Gripper">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Open Gripper"
6+
_description="Open the gripper"
7+
_favorite="true"
8+
>
9+
<Control ID="Sequence" name="root">
10+
<Action
11+
ID="MoveGripperAction"
12+
gripper_command_action_name="/franka_gripper_controller/gripper_cmd"
13+
position="0.04"
14+
timeout="10.000000"
15+
/>
16+
</Control>
17+
</BehaviorTree>
18+
<TreeNodesModel>
19+
<SubTree ID="Open Gripper">
20+
<MetadataFields>
21+
<Metadata subcategory="Grasping" />
22+
<Metadata runnable="true" />
23+
</MetadataFields>
24+
</SubTree>
25+
</TreeNodesModel>
26+
</root>

‎src/moveit_pro_franka_configs/franka_arm_sim/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
<exec_depend>franka_msgs</exec_depend>
1717
<exec_depend>moveit_studio_agent</exec_depend>
1818
<exec_depend>moveit_studio_behavior</exec_depend>
19-
19+
2020
<test_depend>ament_lint_auto</test_depend>
2121

2222
<test_depend>ament_clang_format</test_depend>

0 commit comments

Comments
 (0)