merge with mushr car

rogeriobonatti · Feb 24, 2022 · c926772 · c926772
2 parents 3f5b767 + 5e864b9
commit c926772
Show file tree

Hide file tree

Showing 18 changed files with 2,482 additions and 18 deletions.
diff --git a/mushr_rhc_ros/launch/params/trajgen/dispersion.yaml b/mushr_rhc_ros/launch/params/trajgen/dispersion.yaml
@@ -7,7 +7,7 @@ horizon:
 trajgen:
     dispersion:
         samples: 9
-    desired_speed: 1.0
+    desired_speed: 2.5
     min_delta: -0.38
     max_delta: 0.38
 cost_fn:
@@ -16,4 +16,4 @@ cost_fn:
     smoothing_discount_rate: 0.0
     bounds_cost: 1000.0
 world_rep:
-    epsilon: 0.65
+    epsilon: 0.65
diff --git a/mushr_rhc_ros/launch/params/trajgen/dispersion_language.yaml b/mushr_rhc_ros/launch/params/trajgen/dispersion_language.yaml
@@ -0,0 +1,23 @@
+T: 21
+K: 301
+trajgen_name: 'dispersion'
+horizon:
+    distance: 3.5
+    time: -1.5
+trajgen:
+    dispersion:
+        samples: 9
+    desired_speed: 2.5
+    min_delta: -0.34
+    max_delta: 0.34
+cost_fn:
+    obs_dist_w: 86.0
+    cost2go_w: 0.0
+    smoothing_discount_rate: 0.0
+    bounds_cost: 1000.0
+    direction_penalize_w: 100
+world_rep:
+    epsilon: 0.65
+debug:
+    viz_rollouts:
+        print_stats: False
diff --git a/mushr_rhc_ros/launch/real/record_server.launch b/mushr_rhc_ros/launch/real/record_server.launch
@@ -12,6 +12,7 @@
             args="record -O $(arg record_path)/data.bag
             car_pose
             experiments/finished
+            experiments/language_bias
             mux/ackermann_cmd_mux/active
             mux/ackermann_cmd_mux/input/navigation
             rhcontroller/goal

diff --git a/mushr_rhc_ros/launch/sim/sim_model_deploy.launch b/mushr_rhc_ros/launch/sim/sim_model_deploy.launch
@@ -8,7 +8,8 @@
     </group>
 
     <group ns="$(arg car_name)">
-        <node pkg="mushr_rhc_ros" type="rhcnode_network_shuang.py" name="rhcontroller" output="screen">
+        <node pkg="mushr_rhc_ros" type="rhcnode_network_iros.py" name="rhcontroller" output="screen">
+        <!-- <node pkg="mushr_rhc_ros" type="rhcnode_network_shuang.py" name="rhcontroller" output="screen"> -->
         <!-- <node pkg="mushr_rhc_ros" type="rhcnode_network.py" name="rhcontroller" output="screen"> -->
         <!-- <node pkg="mushr_rhc_ros" type="rhcnode_network_prime.py" name="rhcontroller" output="screen"> -->
             <env name="RHC_USE_CUDA" value="0" />

diff --git a/mushr_rhc_ros/launch/sim/sim_server_eval.launch b/mushr_rhc_ros/launch/sim/sim_server_eval.launch
@@ -0,0 +1,30 @@
+<launch>
+    <arg name="tg" default="dispersion" />
+	<arg name="map_server" default="1" />
+	<arg name="car_name" default="car" />
+
+    <arg name="out_path" default="/home/rb/hackathon_data/e2e_eval/model_test" />
+    <arg name="model_path" default="/home/rb/hackathon_data/aml_outputs/log_output/normal-kingfish/GPTiros_e2e_8gpu_2022-02-17_1645120431.7528405_2022-02-17_1645120431.7528613/model/epoch10.pth.tar" />
+
+    <group if="$(arg map_server)">
+	<include file="$(find mushr_rhc_ros)/launch/map_server.launch" />
+    </group>
+
+    <group ns="$(arg car_name)">
+        <node pkg="mushr_rhc_ros" type="rhcnode_network_iros.py" name="rhcontroller" output="screen">
+            <env name="RHC_USE_CUDA" value="0" />
+
+            <param name="out_path" value="$(arg out_path)" />
+            <param name="model_path" value="$(arg model_path)" />
+
+            <param name="inferred_pose_t" value="car_pose" />
+
+            <param name="car_name" value="$(arg car_name)" />
+
+            <rosparam file="$(find mushr_rhc_ros)/launch/params/trajgen/$(arg tg).yaml" />
+            <rosparam file="$(find mushr_rhc_ros)/launch/params/all_params.yaml" />
+            <rosparam file="$(find mushr_rhc_ros)/launch/sim/params.yaml" />
+        </node>
+    </group>
+
+</launch>
diff --git a/mushr_rhc_ros/launch/sim/sim_server_language.launch b/mushr_rhc_ros/launch/sim/sim_server_language.launch
@@ -0,0 +1,39 @@
+<launch>
+    <arg name="tg" default="dispersion_language" />
+	<arg name="map_server" default="1" />
+	<arg name="car_name" default="car" />
+
+    <group if="$(arg map_server)">
+	<include file="$(find mushr_rhc_ros)/launch/map_server.launch" />
+    </group>
+
+    <group ns="$(arg car_name)">
+        <node pkg="mushr_rhc_ros" type="rhcnode_record_language.py" name="rhcontroller" output="screen">
+            <env name="RHC_USE_CUDA" value="0" />
+
+            <param name="inferred_pose_t" value="car_pose" />
+	        <!-- <param name="inferred_pose_t" value="particle_filter/inferred_pose" /> -->
+
+            <param name="car_name" value="$(arg car_name)" />
+
+            <rosparam file="$(find mushr_rhc_ros)/launch/params/trajgen/$(arg tg).yaml" />
+            <rosparam file="$(find mushr_rhc_ros)/launch/params/all_params.yaml" />
+            <rosparam file="$(find mushr_rhc_ros)/launch/sim/params.yaml" />
+            <!-- <rosparam file="$(find mushr_rhc_ros)/launch/viz-paths/params.yaml" /> -->
+        </node>
+
+        <node pkg="mushr_rhc_ros" type="control_node.py" name="control_node" output="screen" required="true" >
+        </node>
+
+        <!-- <node pkg="mushr_rhc_ros" type="rhcdebug.py" name="rhcdebug" output="screen">
+        <env name="RHC_USE_CUDA" value="0" />
+
+		<param name="inferred_pose_t" value="car_pose" />
+
+        <rosparam file="$(find mushr_rhc_ros)/launch/params/trajgen/$(arg tg).yaml" />
+		<rosparam file="$(find mushr_rhc_ros)/launch/params/all_params.yaml" />
+        <rosparam file="$(find mushr_rhc_ros)/launch/debug/params.yaml" />
+        </node> -->
+
+    </group>
+</launch>
diff --git a/mushr_rhc_ros/launch/viz-paths/viz-paths.launch b/mushr_rhc_ros/launch/viz-paths/viz-paths.launch
@@ -7,7 +7,7 @@
         <env name="RHC_USE_CUDA" value="0" />
 
         <rosparam file="$(find mushr_rhc_ros)/launch/params/trajgen/$(arg tg).yaml" />
-        <rosparam file="$(find mushr_rhc_ros)/launch/all_params.yaml" />
+        <rosparam file="$(find mushr_rhc_ros)/launch/params/all_params.yaml" />
         <rosparam file="$(find mushr_rhc_ros)/launch/viz-paths/params.yaml" />
     </node>
 </launch>
diff --git a/mushr_rhc_ros/src/librhc/cost/waypoints.py b/mushr_rhc_ros/src/librhc/cost/waypoints.py
@@ -28,20 +28,33 @@ def reset(self):
         self.dist_w = self.params.get_float("cost_fn/dist_w", default=1.0)
         self.obs_dist_w = self.params.get_float("cost_fn/obs_dist_w", default=5.0)
         self.cost2go_w = self.params.get_float("cost_fn/cost2go_w", default=1.0)
-        self.smoothing_discount_rate = self.params.get_float(
-            "cost_fn/smoothing_discount_rate", default=0.04
-        )
+        self.smoothing_discount_rate = self.params.get_float("cost_fn/smoothing_discount_rate", default=0.04)
+        self.penalize_right_rate = self.params.get_float("cost_fn/penalize_right", default=0.1)
+        self.penalize_right_w = self.params.get_float("cost_fn/penalize_right_w", default=0)
+        self.penalize_left_rate = self.params.get_float("cost_fn/penalize_left", default=0.1)
+        self.penalize_left_w = self.params.get_float("cost_fn/penalize_left_w", default=0)
         self.bounds_cost = self.params.get_float("cost_fn/bounds_cost", default=100.0)
 
         self.obs_dist_cooloff = torch.arange(1, self.T + 1).mul_(2).type(self.dtype)
 
+        # overall smoothness
         self.discount = self.dtype(self.T - 1)
-
         self.discount[:] = 1 + self.smoothing_discount_rate
         self.discount.pow_(torch.arange(0, self.T - 1).type(self.dtype) * -1)
+
+        # penalize right
+        self.discount_right = self.dtype(self.T)
+        self.discount_right[:] = 1 + self.penalize_right_rate
+        self.discount_right.pow_(torch.arange(0, self.T).type(self.dtype) * -1)
+
+        # penalize left
+        self.discount_left = self.dtype(self.T)
+        self.discount_left[:] = 1 + self.penalize_left_rate
+        self.discount_left.pow_(torch.arange(0, self.T).type(self.dtype) * -1)
+
         self.world_rep.reset()
 
-    def apply(self, poses, goal):
+    def apply(self, poses, goal, trajs):
         """
         Args:
         poses [(K, T, 3) tensor] -- Rollout of T positions
@@ -78,7 +91,15 @@ def apply(self, poses, goal):
             .sum(dim=1)
         )
 
-        result = cost2go.add(collision_cost).add(obs_dist_cost).add(smoothness)
+        trajs_right = trajs[:,:,1].detach().clone()
+        trajs_right[trajs_right>0.0] = 0.0
+        penalize_right = self.penalize_right_w*trajs_right.abs().mul(self.discount_right).sum(dim=1)
+
+        trajs_left = trajs[:,:,1].detach().clone()
+        trajs_left[trajs_left<0.0] = 0.0
+        penalize_left = self.penalize_left_w*trajs_left.abs().mul(self.discount_left).sum(dim=1)
+
+        result = cost2go.add(collision_cost).add(obs_dist_cost).add(smoothness).add(penalize_right).add(penalize_left)
 
         # filter out all colliding trajectories
         colliding = collision_cost.nonzero()

diff --git a/mushr_rhc_ros/src/librhc/mpc.py b/mushr_rhc_ros/src/librhc/mpc.py
@@ -76,7 +76,7 @@ def step(self, state):
             cur_x = self.rollouts[:, t - 1]
             self.rollouts[:, t] = self.kinematics.apply(cur_x, trajs[:, t - 1])
 
-        costs = self.cost.apply(self.rollouts, g)
+        costs = self.cost.apply(self.rollouts, g, trajs)
         result, idx = self.trajgen.generate_control(trajs, costs)
         return result, self.rollouts[idx]