Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
53 changes: 7 additions & 46 deletions src/lab_sim/description/picknik_ur.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@
<material name="plastic">
<color rgba="0.1 0.1 0.1 1" />
</material>
<material name="offwhite">
<color rgba="0.01 0.01 0.01 1" />
</material>
<link name="world" />
<joint name="tool_changer_joint" type="fixed">
<!-- The parent link must be read from the robot model it is attached to. -->
Expand Down Expand Up @@ -1174,57 +1177,15 @@
<!-- Table -->
<link name="table">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.025" />
<geometry>
<mesh
filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae"
scale="0.125 0.125 0.00625"
/>
</geometry>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.4 0.4 -0.2" />
<geometry>
<box size="0.04 0.04 0.45" />
</geometry>
<material name="grey">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.4 -0.4 -0.2" />
<geometry>
<box size="0.04 0.04 0.45" />
</geometry>
<material name="grey">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.4 -0.4 -0.2" />
<origin rpy="0 0 0" xyz="0 0 -0.45" />
<geometry>
<box size="0.04 0.04 0.45" />
<box size="3.5 1 0.9" />
</geometry>
<material name="grey">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.4 0.4 -0.2" />
<geometry>
<box size="0.04 0.04 0.45" />
</geometry>
<material name="grey">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<origin rpy="0 0 0" xyz="0 0 -0.45" />
<geometry>
<mesh
filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae"
scale="0.125 0.125 0.00625"
/>
<box size="3.5 1 0.9" />
</geometry>
</collision>
</link>
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/apriltag_pick_object.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<BehaviorTree
ID="Pick April Tag Labeled Object"
_description="Picks up an object that has an AprilTag marker."
_favorite="false"
_favorite="true"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Look at table" _collapsed="true" />
Expand Down
10 changes: 7 additions & 3 deletions src/lab_sim/objectives/constrained_pick_place.xml
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,10 @@
constraints="{constraints}"
/>
</Decorator>
<!--Pick-->
<SubTree ID="Close Gripper" />
<!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
<Decorator ID="ForceSuccess">
<SubTree ID="Close Gripper" />
</Decorator>
<!--Move to place (drop) location-->
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
Expand Down Expand Up @@ -110,7 +112,9 @@
waypoint_name="Place Cube"
/>
</Decorator>
<SubTree ID="Close Gripper" />
<Decorator ID="ForceSuccess">
<SubTree ID="Close Gripper" />
</Decorator>
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
Expand Down
3 changes: 1 addition & 2 deletions src/lab_sim/objectives/create_pose_vector.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
<!--//////////-->
<BehaviorTree ID="Create Pose Vector">
<BehaviorTree ID="Create Pose Vector" _favorite="false">
<Control ID="Sequence">
<Action
ID="CreateStampedPose"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
/>
<Action
ID="LoadJointTrajectoryFromYaml"
file_path="/home/michael/user_ws/src/lab_sim/objectives/joint_trajectory.yaml"
file_path="joint_trajectory.yaml"
/>
<Action ID="ExecuteFollowJointTrajectory" />
</Control>
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/look_at_table.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<root BTCPP_format="4" main_tree_to_execute="Look at table">
<!--//////////-->
<BehaviorTree ID="Look at table" _description="" _favorite="false">
<BehaviorTree ID="Look at table" _description="" _favorite="true">
<Control ID="Sequence" name="TopLevelSequence">
<SubTree
ID="Move to Waypoint"
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/move_with_velocity_and_force.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<BehaviorTree
ID="Move until Force Contact"
_description="An example for how to use the VFC to move with a velocity / force reference until it collides"
_favorite="false"
_favorite="true"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree
Expand Down
9 changes: 7 additions & 2 deletions src/lab_sim/objectives/place_object.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
<root BTCPP_format="4" main_tree_to_execute="Place object">
<root BTCPP_format="4" main_tree_to_execute="Place Object">
<!--//////////-->
<BehaviorTree ID="Place object" _description="" _favorite="false">
<BehaviorTree
ID="Place Object"
_description=""
_favorite="false"
_subtreeOnly="false"
>
<Control ID="Sequence" name="root">
<Control ID="Sequence">
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
Expand Down
29 changes: 0 additions & 29 deletions src/lab_sim/objectives/record_trajectory.xml

This file was deleted.

2 changes: 1 addition & 1 deletion src/lab_sim/objectives/register_cad_part.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<root BTCPP_format="4" main_tree_to_execute="Register CAD Part">
<!--//////////-->
<BehaviorTree ID="Register CAD Part" _description="" _favorite="false">
<BehaviorTree ID="Register CAD Part" _description="" _favorite="true">
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="ClearSnapshot" />
<SubTree ID="Look at table" _collapsed="true" />
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/take_snap.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<root BTCPP_format="4" main_tree_to_execute="Scene camera snapshot">
<!--//////////-->
<BehaviorTree ID="Scene camera snapshot" _description="" _favorite="false">
<BehaviorTree ID="Scene camera snapshot" _description="" _favorite="true">
<Control ID="Sequence" name="TopLevelSequence">
<!-- Clear out old snapshot data -->
<Action ID="ClearSnapshot" />
Expand Down
2 changes: 1 addition & 1 deletion src/lab_sim/objectives/wrist_snap.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
<!--//////////-->
<BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="false">
<BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
<Control ID="Sequence" name="TopLevelSequence">
<!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
<Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<?xml version="1.0" encoding="UTF-8" ?>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Close Gripper">
<BehaviorTree ID="Close Gripper" _description="Close the gripper">
<Control ID="Sequence" name="root">
Expand All @@ -10,4 +10,7 @@
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Close Gripper" />
</TreeNodesModel>
</root>
2 changes: 1 addition & 1 deletion src/picknik_accessories