Skip to content

New functionality to save the Robot State to file and load it back. #523

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
MikeWrock merged 1 commit into from
Mar 9, 2026
Merged

New functionality to save the Robot State to file and load it back. #523

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
46 changes: 46 additions & 0 deletions src/lab_sim/objectives/load_mujoco_robot_state.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Load MuJoCo Robot State">
<!--//////////-->
<BehaviorTree
ID="Load MuJoCo Robot State"
_description="Load the Robot State from file and update the Robot State in the MuJoCo simulation."
_favorite="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<Action
ID="ListControllers"
type="command"
controller_list="{command_controllers}"
state="active"
/>
<Decorator ID="ForceSuccess">
<Action
ID="SwitchController"
deactivate_controllers="{command_controllers}"
/>
</Decorator>
<Action
ID="LoadRobotJointStateFromYaml"
output="{joint_state_msg}"
file_path="robot_joint_state.yaml"
/>
<Decorator ID="ForceSuccess">
<Action ID="SetMujocoState" />
</Decorator>
<Decorator ID="ForceSuccess">
<Action
ID="SwitchController"
activate_controllers="{command_controllers}"
/>
</Decorator>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Load MuJoCo Robot State">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="MuJoCo Simulation" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
6 changes: 3 additions & 3 deletions src/lab_sim/objectives/reset_mujoco_robot.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Reset MuJoco Robot">
<root BTCPP_format="4" main_tree_to_execute="Reset MuJoCo Robot">
<!--//////////-->
<BehaviorTree
ID="Reset MuJoco Robot"
ID="Reset MuJoCo Robot"
_description="Set the joint state of the robot in the MuJoCo simulation."
_favorite="false"
>
Expand Down Expand Up @@ -32,7 +32,7 @@
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Reset MuJoco Robot">
<SubTree ID="Reset MuJoCo Robot">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="MuJoCo Simulation" />
Expand Down
48 changes: 48 additions & 0 deletions src/lab_sim/objectives/robot_joint_state.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
joint_state:
header:
stamp:
sec: 1772563087
nanosec: 123736969
frame_id: ""
name:
- linear_rail_joint
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
position:
- -3.7888509479240455e-13
- 7.7000625282890057e-10
- -1.8849000070075239
- 1.0996999957226834
- -1.2566000010799672
- -1.5707500000002714
- -5.6739939771275992e-13
velocity:
- 0
- 0
- 0
- 0
- 0
- 0
- 0
effort:
- 0
- 0
- 0
- 0
- 0
- 0
- 0
multi_dof_joint_state:
header:
stamp:
sec: 0
nanosec: 0
frame_id: world
joint_names: []
transforms: []
twist: []
wrench: []
23 changes: 23 additions & 0 deletions src/lab_sim/objectives/save_robot_joint_state.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Save Robot Joint State">
<!--//////////-->
<BehaviorTree ID="Save Robot Joint State" _description="" _favorite="false">
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="GetCurrentPlanningScene" />
<Action ID="GetRobotJointState" />
<Action
ID="SaveRobotJointStateToYaml"
message="{robot_joint_state}"
yaml_filename="robot_joint_state"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Save Robot Joint State">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="MuJoCo Simulation" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
6 changes: 3 additions & 3 deletions src/lab_sim/objectives/show_grasp_link.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="show grasp_link">
<root BTCPP_format="4" main_tree_to_execute="Show grasp_link">
<!--//////////-->
<BehaviorTree ID="show grasp_link" _description="" _favorite="false">
<BehaviorTree ID="Show grasp_link" _description="" _favorite="false">
<Control ID="Sequence" name="TopLevelSequence">
<SubTree
ID="Get Transform Frame Pose"
Expand All @@ -26,7 +26,7 @@
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="show grasp_link">
<SubTree ID="Show grasp_link">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="Application - Advanced Examples" />
Expand Down