Skip to content

Commit 678b999

Browse files
rockettmrockettm
committed
Robust? cost function in sim with pf
1 parent 5b401fd commit 678b999

File tree

12 files changed

+331
-40
lines changed

12 files changed

+331
-40
lines changed

‎.gitignore

+2
Original file line numberDiff line numberDiff line change
@@ -8,3 +8,5 @@ __pycache__/
88

99
*.swp
1010
*.swo
11+
12+
*.bag

‎mushr_rhc_ros/debugger.rviz

+208
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
Panels:
2+
- Class: rviz/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Global Options1
8+
- /Status1
9+
- /LaserScan1
10+
- /Pose1
11+
- /Pose1/Status1
12+
- /Marker1
13+
- /Marker2
14+
- /MarkerArray1
15+
- /MarkerArray1/Namespaces1
16+
- /Marker3
17+
- /Marker3/Namespaces1
18+
Splitter Ratio: 0.5
19+
Tree Height: 565
20+
- Class: rviz/Selection
21+
Name: Selection
22+
- Class: rviz/Tool Properties
23+
Expanded:
24+
- /2D Pose Estimate1
25+
- /2D Nav Goal1
26+
- /Publish Point1
27+
Name: Tool Properties
28+
Splitter Ratio: 0.588679
29+
- Class: rviz/Views
30+
Expanded:
31+
- /Current View1
32+
Name: Views
33+
Splitter Ratio: 0.5
34+
- Class: rviz/Time
35+
Experimental: false
36+
Name: Time
37+
SyncMode: 0
38+
SyncSource: LaserScan
39+
Visualization Manager:
40+
Class: ""
41+
Displays:
42+
- Alpha: 0.5
43+
Cell Size: 1
44+
Class: rviz/Grid
45+
Color: 160; 160; 164
46+
Enabled: true
47+
Line Style:
48+
Line Width: 0.03
49+
Value: Lines
50+
Name: Grid
51+
Normal Cell Count: 0
52+
Offset:
53+
X: 0
54+
Y: 0
55+
Z: 0
56+
Plane: XY
57+
Plane Cell Count: 10
58+
Reference Frame: <Fixed Frame>
59+
Value: true
60+
- Alpha: 0.7
61+
Class: rviz/Map
62+
Color Scheme: map
63+
Draw Behind: false
64+
Enabled: true
65+
Name: Map
66+
Topic: /map
67+
Unreliable: false
68+
Value: true
69+
- Alpha: 1
70+
Autocompute Intensity Bounds: true
71+
Autocompute Value Bounds:
72+
Max Value: 10
73+
Min Value: -10
74+
Value: true
75+
Axis: Z
76+
Channel Name: intensity
77+
Class: rviz/LaserScan
78+
Color: 245; 131; 255
79+
Color Transformer: FlatColor
80+
Decay Time: 0
81+
Enabled: true
82+
Invert Rainbow: false
83+
Max Color: 255; 255; 255
84+
Max Intensity: 4096
85+
Min Color: 0; 0; 0
86+
Min Intensity: 0
87+
Name: LaserScan
88+
Position Transformer: XYZ
89+
Queue Size: 10
90+
Selectable: true
91+
Size (Pixels): 3
92+
Size (m): 0.05
93+
Style: Flat Squares
94+
Topic: /scan
95+
Unreliable: false
96+
Use Fixed Frame: true
97+
Use rainbow: true
98+
Value: true
99+
- Alpha: 1
100+
Axes Length: 1
101+
Axes Radius: 0.1
102+
Class: rviz/Pose
103+
Color: 255; 25; 0
104+
Enabled: true
105+
Head Length: 0.3
106+
Head Radius: 0.1
107+
Name: Pose
108+
Shaft Length: 1
109+
Shaft Radius: 0.05
110+
Shape: Arrow
111+
Topic: /pf/ta/viz/inferred_pose
112+
Unreliable: false
113+
Value: true
114+
- Class: rviz/Marker
115+
Enabled: true
116+
Marker Topic: /rhcdebug/current_path
117+
Name: Marker
118+
Namespaces:
119+
"": true
120+
Queue Size: 100
121+
Value: true
122+
- Class: rviz/Marker
123+
Enabled: true
124+
Marker Topic: /rhcdebug/traj_chosen
125+
Name: Marker
126+
Namespaces:
127+
"": true
128+
Queue Size: 100
129+
Value: true
130+
- Class: rviz/MarkerArray
131+
Enabled: true
132+
Marker Topic: /rhcdebug/debug/viz_rollouts
133+
Name: MarkerArray
134+
Namespaces:
135+
collision_cost: false
136+
cost2go: false
137+
final_result: true
138+
obstacle_dist_cost: false
139+
smoothness: false
140+
Queue Size: 100
141+
Value: true
142+
- Class: rviz/Marker
143+
Enabled: true
144+
Marker Topic: /rhcdebug/value_fn
145+
Name: Marker
146+
Namespaces:
147+
{}
148+
Queue Size: 100
149+
Value: true
150+
Enabled: true
151+
Global Options:
152+
Background Color: 48; 48; 48
153+
Fixed Frame: map
154+
Frame Rate: 30
155+
Name: root
156+
Tools:
157+
- Class: rviz/Interact
158+
Hide Inactive Objects: true
159+
- Class: rviz/MoveCamera
160+
- Class: rviz/Select
161+
- Class: rviz/FocusCamera
162+
- Class: rviz/Measure
163+
- Class: rviz/SetInitialPose
164+
Topic: /initialpose
165+
- Class: rviz/SetGoal
166+
Topic: /move_base_simple/goal
167+
- Class: rviz/PublishPoint
168+
Single click: true
169+
Topic: /clicked_point
170+
Value: true
171+
Views:
172+
Current:
173+
Class: rviz/Orbit
174+
Distance: 6.75186
175+
Enable Stereo Rendering:
176+
Stereo Eye Separation: 0.06
177+
Stereo Focal Distance: 1
178+
Swap Stereo Eyes: false
179+
Value: false
180+
Focal Point:
181+
X: 30.5168
182+
Y: 8.47859
183+
Z: 8.47257
184+
Name: Current View
185+
Near Clip Distance: 0.01
186+
Pitch: 1.5498
187+
Target Frame: <Fixed Frame>
188+
Value: Orbit (rviz)
189+
Yaw: 1.5754
190+
Saved: ~
191+
Window Geometry:
192+
Displays:
193+
collapsed: false
194+
Height: 846
195+
Hide Left Dock: false
196+
Hide Right Dock: false
197+
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f80000003efc0100000002fb0000000800540069006d00650100000000000004f8000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000388000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
198+
Selection:
199+
collapsed: false
200+
Time:
201+
collapsed: false
202+
Tool Properties:
203+
collapsed: false
204+
Views:
205+
collapsed: false
206+
Width: 1272
207+
X: 79
208+
Y: 41

‎mushr_rhc_ros/launch/debug/params.yaml

+2-5
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,4 @@
11
traj_chosen_topic: '/rhcontroller/traj_chosen'
2-
use_sim_pose: false
3-
use_odom_pose: true
42

53
debug:
64
ip_topic: '/sim_car_pose/pose'
@@ -10,8 +8,7 @@ debug:
108
viz_traj_chosen: true
119
viz_cost_fn: true
1210
viz_rollouts:
13-
n: 1
11+
n: -1
1412
print_stats: false
1513
traj_chosen_trace:
16-
rate: 4
17-
14+
rate: 1

‎mushr_rhc_ros/launch/params/trajgen/dispersion.yaml

+17-8
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,19 @@
11
T: 21
2-
K: 121
2+
K: 201
33
trajgen_name: 'dispersion'
4-
horizon/distance: 3.0
5-
horizon/time: -1.5
6-
trajgen/dispersion/samples: 7
7-
trajgen/desired_speed: 2.0
8-
trajgen/min_delta: -.34
9-
trajgen/max_delta: .34
10-
worldrep/epsilon: 0.5
4+
horizon:
5+
distance: 4.0
6+
time: -1.5
7+
trajgen:
8+
dispersion:
9+
samples: 9
10+
desired_speed: 3.0
11+
min_delta: -0.34
12+
max_delta: 0.34
13+
cost_fn:
14+
obs_dist_w: 1.0
15+
cost2go_w: 50.0
16+
smoothing_discount_rate: 0.05
17+
bounds_cost: 1000.0
18+
world_rep:
19+
epsilon: 0.5

‎mushr_rhc_ros/launch/real/params.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
use_sim_pose: false
22
use_odom_pose: true
33

4+
profile: false
45
debug: {}

‎mushr_rhc_ros/launch/real/real.launch

+1-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
<include file="$(find mushr_rhc_ros)/launch/mapserver.launch" />
55

6-
<node pkg="mushr_rhc_ros" type="main.py" name="rhcontroller" output="screen">
6+
<node pkg="mushr_rhc_ros" type="rhcnode.py" name="rhcontroller" output="screen">
77
<env name="RHC_USE_CUDA" value="0" />
88

99
<param name="cache_dir" value="~/mushr_rhc_cache/" />

‎mushr_rhc_ros/src/librhc/cost/waypoints.py

+8-10
Original file line numberDiff line numberDiff line change
@@ -49,23 +49,22 @@ def apply(self, poses, goal):
4949
all_poses = poses.view(self.K * self.T, self.NPOS)
5050

5151
# use terminal distance (K, tensor)
52-
dists = poses[:, self.T-1, :2].sub(goal[:2]).norm(p=2, dim=1).mul(self.dist_w)
5352
cost2go = self.value_fn.get_value(poses[:, self.T-1, :]).mul(self.cost2go_w)
5453

5554
# get all collisions (K, T, tensor)
56-
collisions = self.world_rep.check_collision_in_map(
57-
all_poses).view(self.K, self.T)
55+
collisions = self.world_rep.check_collision_in_map(all_poses).view(self.K, self.T)
56+
collision_cost = collisions.sum(dim=1).mul(self.bounds_cost)
5857

59-
obstacle_distances = self.world_rep.distances(
60-
all_poses).view(self.K, self.T)
58+
obstacle_distances = self.world_rep.distances(all_poses).view(self.K, self.T)
6159

62-
collision_cost = collisions.sum(dim=1).mul(self.bounds_cost)
63-
obstacle_dist_cost = obstacle_distances[:].sum(dim=1).mul(self.obs_dist_w)
60+
obs_dist_cost = obstacle_distances[:].sum(dim=1).mul(self.obs_dist_w)
6461

6562
# reward smoothness by taking the integral over the rate of change in poses,
6663
# with time-based discounting factor
6764
smoothness = ((poses[:, 1:, 2] - poses[:, :self.T-1, 2])).abs().mul(self.discount).sum(dim=1)
68-
result = dists.add(cost2go).add(collision_cost).add(obstacle_dist_cost).add(smoothness)
65+
66+
# result = dists.add(cost2go).add(collision_cost).add(obstacle_dist_cost).add(smoothness)
67+
result = cost2go.add(collision_cost).add(obs_dist_cost).add(smoothness)
6968

7069
if self.viz_rollouts:
7170
import librhc.rosviz as rosviz
@@ -81,10 +80,9 @@ def print_n(c, ns, cmap='coolwarm'):
8180
", Avg: " + str(torch.mean(c)))
8281

8382
print_n(result, ns="final_result")
84-
print_n(dists, ns="dists")
8583
print_n(cost2go, ns="cost2go")
8684
print_n(collision_cost, ns="collision_cost")
87-
print_n(obstacle_dist_cost, ns="obstacle_dist_cost")
85+
print_n(obs_dist_cost, ns="obstacle_dist_cost")
8886
print_n(smoothness, ns="smoothness")
8987

9088
return result

‎mushr_rhc_ros/src/librhc/mpc.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ def step(self, state):
4848
assert state.size() == (3,)
4949

5050
if self.at_goal(state):
51-
return None
51+
return None, None
5252

5353
with self.goal_lock:
5454
g = self.goal

‎mushr_rhc_ros/src/librhc/value/simpleknn.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,7 @@ def _iterative_sample_seq(self, h, w, threshold=300, sampler=None):
9191
print "valid points len: " + str(len(valid))
9292
return np.array(valid)
9393

94-
def set_goal(self, goal, n_neighbors=7, k=3):
94+
def set_goal(self, goal, n_neighbors=7, k=6):
9595
"""
9696
Args:
9797
goal [(3,) tensor] -- Goal in "world" coordinates

‎mushr_rhc_ros/src/librhc/worldrep/simple.py

+2
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,8 @@ def collisions(self, poses):
5959

6060
xs = self.scaled[:, 0].long()
6161
ys = self.scaled[:, 1].long()
62+
print xs
63+
print ys
6264

6365
self.perm.zero_()
6466
self.perm |= self.perm_reg[ys, xs]

0 commit comments

Comments
 (0)