Skip to content

Update PublishVelocityForceCommand to use unstamped twist/wrench #638

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
marioprats merged 2 commits into from
May 19, 2026
Merged

Update PublishVelocityForceCommand to use unstamped twist/wrench #638

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9ff6820
Update PublishVelocityForceCommand to use unstamped twist/wrench
marioprats May 18, 2026
8356e45
Merge branch 'main' into update-publish-velocity-force-command
marioprats May 18, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 25 additions & 9 deletions src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -210,15 +210,31 @@
hand_frame_name="grasp_link"
force_threshold="100"
/>
<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="0;0;0;0;0;0"
wrench_gain="0.01"
velocity_controlled_axes="0;0;1;0;0;0"
publish_rate="50"
twist_stamped="{desired_twist}"
wrench_stamped="{desired_wrench}"
/>
<Control ID="Sequence" name="Sequence">
<Action
ID="UnpackTwistStampedMessage"
name="UnpackTwistStampedMessage"
twist="{twist}"
header="{header}"
twist_stamped="{desired_twist}"
/>
<Action
ID="UnpackWrenchStampedMessage"
name="UnpackWrenchStampedMessage"
wrench="{wrench}"
header="{header}"
wrench_stamped="{desired_wrench}"
/>
Comment on lines +213 to +227
Copy link
Copy Markdown
Collaborator

@shaur-k shaur-k May 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same q here if we can just use the already existing Unpack Behaviors

Copy link
Copy Markdown
Author

@marioprats marioprats May 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Switched to use Unpack Behaviors

<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="0;0;0;0;0;0"
wrench_gain="0.01"
velocity_controlled_axes="0;0;1;0;0;0"
publish_rate="50"
twist="{twist}"
wrench="{wrench}"
/>
</Control>
</Control>
<SubTree
ID="Force Relaxation"
Expand Down
29 changes: 23 additions & 6 deletions src/lab_sim/objectives/force_relaxation.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Force Relaxation">
<!--//////////-->
<BehaviorTree
ID="Force Relaxation"
_description="Relaxes the force sensed at the force/torque sensor by moving the robot tip in the direction of the sensed force."
Expand All @@ -11,11 +10,29 @@
ID="CreateTwistStamped"
reference_frame="{tip_link}"
twist_stamped="{zero_twist}"
angular_velocity_xyz="0.000000;0.000000;0.000000"
linear_velocity_xyz="0.000000;0.000000;0.000000"
/>
<Action
ID="UnpackTwistStampedMessage"
name="UnpackTwistStampedMessage"
twist="{twist}"
header="{header}"
twist_stamped="{zero_twist}"
/>
<Action
ID="CreateWrenchStamped"
reference_frame="{tip_link}"
wrench_stamped="{zero_wrench}"
torque_xyz="0.000000;0.000000;0.000000"
force_xyz="0.000000;0.000000;0.000000"
/>
<Action
ID="UnpackWrenchStampedMessage"
name="UnpackWrenchStampedMessage"
wrench="{wrench}"
header="{header}"
wrench_stamped="{zero_wrench}"
/>
<Decorator ID="ForceSuccess">
<Decorator ID="Timeout" msec="{relaxation_time_ms}">
Expand All @@ -24,10 +41,10 @@
wrench_gain="{wrench_gain}"
force_controlled_axes="1;1;1;0;0;0"
publish_rate="50"
twist_stamped="{zero_twist}"
wrench_stamped="{zero_wrench}"
velocity_controlled_axes="0;0;0;0;0;0"
velocity_force_controller_command_topic="/velocity_force_controller/command"
wrench="{wrench}"
twist="{twist}"
/>
</Decorator>
</Decorator>
Expand All @@ -39,9 +56,9 @@
force_controlled_axes="0;0;0;0;0;0"
publish_rate="50"
velocity_controlled_axes="1;1;1;0;0;0"
wrench_stamped="{zero_wrench}"
twist_stamped="{zero_twist}"
velocity_force_controller_command_topic="/velocity_force_controller/command"
wrench="{wrench}"
twist="{twist}"
/>
</Decorator>
</Decorator>
Expand All @@ -50,8 +67,8 @@
<TreeNodesModel>
<SubTree ID="Force Relaxation">
<MetadataFields>
<Metadata subcategory="Motion - Controls" />
<Metadata runnable="false" />
<Metadata subcategory="Motion - Controls" />
</MetadataFields>
<input_port name="relaxation_time_ms" default="500" />
<input_port name="tip_link" default="grasp_link" />
Expand Down
72 changes: 62 additions & 10 deletions src/lab_sim/objectives/move_with_velocity_and_force.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Push Button">
<!--//////////-->
<BehaviorTree
ID="Push Button"
_description="An example for how to use the VFC to move with a velocity / force reference until it collides"
Expand All @@ -13,14 +12,33 @@
_collapsed="true"
waypoint_name="VFC Start"
joint_group_name="manipulator"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_admittance_controller/follow_joint_trajectory"
controller_names="joint_trajectory_admittance_controller"
execution_pipeline="jtac"
keep_orientation="false"
keep_orientation_tolerance="0.05"
link_padding="0.01"
seed="0"
velocity_scale_factor="1.0"
/>
<Action
ID="SwitchController"
activate_controllers="velocity_force_controller"
controller_switch_action_name="/controller_manager/switch_controller"
timeout="-1.000000"
activate_asap="true"
strictness="1"
controller_list_action_name="/controller_manager/list_controllers"
automatic_deactivation="true"
automatic_activation="true"
deactivate_controllers=""
/>
<Action
ID="CallTriggerService"
service_name="/velocity_force_controller/zero_fts"
response_timeout="3.000000"
wait_for_server_available_timeout="3.000000"
/>
<Action
ID="CreateWrenchStamped"
Expand All @@ -40,11 +58,14 @@
ID="CreatePoseStamped"
reference_frame="grasp_link"
pose_stamped="{initial_pose}"
orientation_xyzw="0.000000;0.000000;0.000000;1.000000"
position_xyz="0.000000;0.000000;0.000000"
/>
<Action
ID="TransformPoseFrame"
output_pose="{initial_pose}"
input_pose="{initial_pose}"
target_frame_id="world"
/>
<Control ID="Parallel" failure_count="1" success_count="1">
<Action
Expand All @@ -53,34 +74,65 @@
wrench_frame_name="tool0"
hand_frame_name="grasp_link"
force_threshold="20"
wrench_topic_name="/force_torque_sensor_broadcaster/wrench"
/>
<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="0;0;0;0;0;0"
wrench_gain="0.01"
velocity_controlled_axes="0;0;1;0;0;0"
publish_rate="50"
/>
<Control ID="Sequence" name="Sequence">
<Action
ID="UnpackTwistStampedMessage"
name="UnpackTwistStampedMessage"
twist="{twist}"
header="{header}"
twist_stamped="{desired_twist}"
/>
<Action
ID="UnpackWrenchStampedMessage"
name="UnpackWrenchStampedMessage"
wrench="{wrench}"
header="{header}"
wrench_stamped="{desired_wrench}"
/>
<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="0;0;0;0;0;0"
wrench_gain="0.01"
velocity_controlled_axes="0;0;1;0;0;0"
publish_rate="50"
wrench="{wrench}"
twist="{twist}"
velocity_force_controller_command_topic="/velocity_force_controller/command"
/>
</Control>
</Control>
<!--Relax the arm for 500ms to just stay in contact without applying force.-->
<SubTree
ID="Force Relaxation"
name="Force Relaxation"
_collapsed="true"
wrench_gain="0.01"
relaxation_time_ms="500"
tip_link="grasp_link"
wrench_gain="0.01"
/>
<!--Relax the arm for 500ms to just stay in contact without applying force.-->
<SubTree
ID="Move to Waypoint"
_collapsed="true"
joint_group_name="manipulator"
waypoint_name="Look at Table"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_admittance_controller/follow_joint_trajectory"
controller_names="joint_trajectory_admittance_controller"
execution_pipeline="jtac"
keep_orientation="false"
keep_orientation_tolerance="0.05"
link_padding="0.01"
seed="0"
velocity_scale_factor="1.0"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Push Button">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="Application - Advanced Examples" />
</MetadataFields>
</SubTree>
Expand Down
36 changes: 26 additions & 10 deletions src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -316,16 +316,32 @@
force_threshold="30"
wrench_topic_name="/force_torque_sensor_broadcaster/wrench"
/>
<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="0;0;0;0;0;0"
wrench_gain="0.01"
velocity_controlled_axes="0;0;1;0;0;0"
publish_rate="50"
twist_stamped="{desired_twist}"
wrench_stamped="{desired_wrench}"
velocity_force_controller_command_topic="/velocity_force_controller/command"
/>
<Control ID="Sequence" name="Sequence">
<Action
ID="UnpackTwistStampedMessage"
name="UnpackTwistStampedMessage"
twist="{twist}"
header="{header}"
twist_stamped="{desired_twist}"
/>
<Action
ID="UnpackWrenchStampedMessage"
name="UnpackWrenchStampedMessage"
wrench="{wrench}"
header="{header}"
wrench_stamped="{desired_wrench}"
/>
<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="0;0;0;0;0;0"
wrench_gain="0.01"
velocity_controlled_axes="0;0;1;0;0;0"
publish_rate="50"
twist="{twist}"
wrench="{wrench}"
velocity_force_controller_command_topic="/velocity_force_controller/command"
/>
</Control>
</Control>
<SubTree
ID="Force Relaxation"
Expand Down
18 changes: 16 additions & 2 deletions src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti_vfc.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,23 +267,37 @@
twist_stamped="{desired_twist}"
reference_frame="world"
/>
<Action
ID="UnpackTwistStampedMessage"
name="UnpackTwistStampedMessage"
twist="{twist}"
header="{header}"
twist_stamped="{desired_twist}"
/>
<Action
ID="CreateWrenchStamped"
torque_xyz="0.0;0.0;0.0"
force_xyz="0.0;0.0;0.0"
wrench_stamped="{desired_wrench}"
reference_frame="world"
/>
<Action
ID="UnpackWrenchStampedMessage"
name="UnpackWrenchStampedMessage"
wrench="{wrench}"
header="{header}"
wrench_stamped="{desired_wrench}"
/>
<Control ID="Parallel" success_count="1" failure_count="1">
<Action
ID="PublishVelocityForceCommand"
force_controlled_axes="1;1;1;1;1;1"
publish_rate="10"
twist_stamped="{desired_twist}"
twist="{twist}"
velocity_controlled_axes="0;0;0;0;0;0"
velocity_force_controller_command_topic="/velocity_force_controller/command"
wrench_gain="0.0"
wrench_stamped="{desired_wrench}"
wrench="{wrench}"
/>
<SubTree ID="Close Gripper" _collapsed="true" />
</Control>
Expand Down
16 changes: 16 additions & 0 deletions src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,13 @@
angular_velocity_xyz="0.0;0.0;0.0"
linear_velocity_xyz="0.0;0.0;0.0"
/>
<Action
ID="UnpackTwistStampedMessage"
name="UnpackTwistStampedMessage"
twist="{twist}"
header="{header}"
twist_stamped="{desired_twist}"
/>
<Action
ID="SwitchController"
activate_controllers="velocity_force_controller"
Expand All @@ -23,12 +30,21 @@
force_xyz="0.0;0.0;0.0"
wrench_stamped="{desired_wrench}"
/>
<Action
ID="UnpackWrenchStampedMessage"
name="UnpackWrenchStampedMessage"
wrench="{wrench}"
header="{header}"
wrench_stamped="{desired_wrench}"
/>
<Decorator ID="KeepRunningUntilFailure">
<Action
ID="PublishVelocityForceCommand"
wrench_gain="0.1"
velocity_controlled_axes="0;0;0;0;0;0"
force_controlled_axes="1;1;1;0;0;0"
twist="{twist}"
wrench="{wrench}"
/>
</Decorator>
</Control>
Expand Down
Loading
Loading