Skip to content

Commit 8ae9a4e

Browse files
bkanatorbkanator
committed
Merge remote-tracking branch 'origin/main' into lidar-hangar-sim
2 parents bcb7688 + f064b5d commit 8ae9a4e

22 files changed

Lines changed: 172 additions & 140 deletions

‎src/hangar_sim/description/hangar_scene.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
</visual>
1515

1616
<worldbody>
17-
<site
17+
<site
1818
name="scene_camera_optical_frame"
1919
pos="-14.4207 -4.39686 9.04094"
2020
quat="0 0.7071 -0.7071 0"

‎src/hangar_sim/description/ur5e_ridgeback.xml‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -211,15 +211,15 @@
211211
<body name="base_platform" pos="0 0 0" euler="0 0 0">
212212
<body name="ridgeback_base_link">
213213
<!-- Front SICK TIM571: mounted at x=+0.45m (front bumper), z=0.15m.
214-
31 beams × 9 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg.
214+
91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg.
215215
quat="0.6533 0.2706 -0.6533 0.2706": beam-0 +Z fires at -135 deg from +X. -->
216216
<body name="lidar_front_mount" pos="0.45 0 0.15">
217217
<inertial
218218
mass="0.25"
219219
pos="0 0 0"
220220
diaginertia="0.0001 0.0001 0.0001"
221221
/>
222-
<replicate count="31" sep="-" offset="0 0 0" euler="0 0 0.15708">
222+
<replicate count="91" sep="-" offset="0 0 0" euler="0 0 0.05236">
223223
<site
224224
name="lidar_front"
225225
size="0.01"
@@ -252,15 +252,15 @@
252252
</body>
253253

254254
<!-- Rear SICK TIM571: mounted at x=-0.45m (rear bumper), z=0.15m.
255-
31 beams × 9 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg.
255+
91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg.
256256
quat="0.2706 -0.6533 -0.2706 -0.6533": beam-0 +Z fires at +45 deg from +X. -->
257257
<body name="lidar_rear_mount" pos="-0.45 0 0.15">
258258
<inertial
259259
mass="0.25"
260260
pos="0 0 0"
261261
diaginertia="0.0001 0.0001 0.0001"
262262
/>
263-
<replicate count="31" sep="-" offset="0 0 0" euler="0 0 0.15708">
263+
<replicate count="91" sep="-" offset="0 0 0" euler="0 0 0.05236">
264264
<site
265265
name="lidar_rear"
266266
size="0.01"
@@ -669,15 +669,15 @@
669669
<framequat objtype="site" objname="imu_site" />
670670
<gyro site="imu_site" />
671671
<accelerometer site="imu_site" />
672-
<!-- Front TIM571: 270 deg FOV, 9 deg resolution (31 beams), 0.05-25m range -->
672+
<!-- Front TIM571: 270 deg FOV, 3 deg resolution (91 beams), 0.05-25m range -->
673673
<rangefinder
674674
site="lidar_front"
675-
user="0.05 -2.35619 2.35619 0.15708 0.05 25.0"
675+
user="0.05 -2.35619 2.35619 0.05236 0.05 25.0"
676676
/>
677677
<!-- Rear TIM571: same specs, mounted facing backward for 360 deg coverage -->
678678
<rangefinder
679679
site="lidar_rear"
680-
user="0.05 -2.35619 2.35619 0.15708 0.05 25.0"
680+
user="0.05 -2.35619 2.35619 0.05236 0.05 25.0"
681681
/>
682682
</sensor>
683683
</mujoco>

‎src/hangar_sim/script/scan_mirror_fix.py‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,12 @@
1919
import rclpy
2020
from rclpy.node import Node
2121
from sensor_msgs.msg import LaserScan
22-
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
22+
from rclpy.qos import (
23+
QoSProfile,
24+
QoSReliabilityPolicy,
25+
QoSDurabilityPolicy,
26+
QoSHistoryPolicy,
27+
)
2328

2429

2530
BEST_EFFORT_QOS = QoSProfile(
@@ -33,7 +38,9 @@
3338
class ScanMirrorFix(Node):
3439
def __init__(self):
3540
super().__init__("scan_mirror_fix")
36-
self.sub = self.create_subscription(LaserScan, "/scan", self.callback, BEST_EFFORT_QOS)
41+
self.sub = self.create_subscription(
42+
LaserScan, "/scan", self.callback, BEST_EFFORT_QOS
43+
)
3744
self.pub = self.create_publisher(LaserScan, "/scan_mirrored", BEST_EFFORT_QOS)
3845

3946
def callback(self, msg):

‎src/lab_sim/config/control/picknik_ur.ros2_control.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,7 @@ robotiq_gripper_controller:
9292
joint: robotiq_85_left_knuckle_joint
9393
allow_stalling: true
9494
stall_timeout: 0.05
95+
stall_velocity_threshold: 0.004
9596
goal_tolerance: 0.02
9697

9798
force_torque_sensor_broadcaster:

‎src/lab_sim/objectives/_scan_scene.xml‎

Lines changed: 0 additions & 34 deletions
This file was deleted.

src/lab_sim/objectives/compute_linkpose_from_waypoint.xml renamed to src/lab_sim/objectives/computelinkposefromwaypoint.xml

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,17 @@
11
<?xml version="1.0" encoding="UTF-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="ComputeLinkPoseFromWaypoint">
33
<!--//////////-->
4-
<BehaviorTree ID="ComputeLinkPoseFromWaypoint" _favorite="false">
4+
<BehaviorTree
5+
ID="ComputeLinkPoseFromWaypoint"
6+
_favorite="false"
7+
_description=""
8+
>
59
<Control ID="Sequence" _collapsed="false">
610
<Action
711
ID="RetrieveWaypoint"
812
joint_group_name="manipulator"
913
waypoint_joint_state="{target_joint_state}"
10-
waypoint_name="Above Pick Cube"
14+
waypoint_name="Above Pick Object"
1115
/>
1216
<Action
1317
ID="UnpackRobotJointStateMessage"
@@ -32,7 +36,7 @@
3236
<SubTree ID="ComputeLinkPoseFromWaypoint">
3337
<MetadataFields>
3438
<Metadata runnable="false" />
35-
<Metadata subcategory="Application - Basic Examples" />
39+
<Metadata subcategory="Manipulation" />
3640
<Metadata
3741
description="This objective retrieves a named waypoint, unpacks the joint state, gets the current planning scene, and computes the pose of a specified link using forward kinematics. The resulting pose is output as a geometry_msgs::msg::PoseStamped."
3842
/>

‎src/lab_sim/objectives/grasp_planning.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
<Control ID="Sequence" name="TopLevelSequence">
1010
<SubTree ID="Clear Snapshot" _collapsed="true" />
1111
<SubTree ID="Look at Table" _collapsed="true" />
12-
<!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
12+
<!--Take a snapshot so we have a visualization of the objects to use in grasp pose correction-->
1313
<SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
1414
<SubTree ID="Open Gripper" _collapsed="true" />
1515
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />

‎src/lab_sim/objectives/mpc_pose_tracking.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
ID="Move to Waypoint"
1313
_collapsed="true"
1414
joint_group_name="manipulator"
15-
waypoint_name="Above Pick Cube"
15+
waypoint_name="Above Pick Object"
1616
/>
1717
<Action
1818
ID="CreateStampedPose"

‎src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
>
1111
<Control ID="Sequence" name="TopLevelSequence">
1212
<SubTree ID="Clear Snapshot" _collapsed="true" />
13-
<SubTree ID="_Scan Scene" _collapsed="true" />
13+
<SubTree ID="Scan Scene - Multiple Point Clouds" _collapsed="true" />
1414
<Action
1515
ID="GetPointCloud"
1616
message_out="{point_cloud}"

‎src/lab_sim/objectives/open_gripper.xml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,19 @@
1-
<?xml version="1.0" ?>
1+
<?xml version="1.0" encoding="UTF-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Open Gripper">
33
<!--//////////-->
44
<BehaviorTree
55
ID="Open Gripper"
66
_description="Open the gripper"
77
_favorite="false"
88
>
9-
<Control ID="Sequence" name="root">
9+
<Decorator ID="ForceSuccess">
1010
<Action
1111
ID="MoveGripperAction"
1212
gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
1313
position="0.05"
1414
timeout="15.000000"
1515
/>
16-
</Control>
16+
</Decorator>
1717
</BehaviorTree>
1818
<TreeNodesModel>
1919
<SubTree ID="Open Gripper">

0 commit comments

Comments
 (0)