Test with 2 turtles, but need better steering controller (to go in st…

…raight line)

docker-compose.yml

@@ -6,6 +6,8 @@ services:
     build:
       context: .
       dockerfile: .docker/Dockerfile
+    environment:
+      - ROS_HOSTNAME=192.168.0.166
     network_mode: host
     privileged: true
     stdin_open: true # docker run -i
@@ -17,6 +19,8 @@ services:
     build:
       context: .
       dockerfile: .docker/Dockerfile
+    environment:
+      - ROS_HOSTNAME=192.168.0.166
     network_mode: host
     privileged: true
     stdin_open: true # docker run -i
@@ -30,6 +34,8 @@ services:
     build:
       context: .
       dockerfile: .docker/Dockerfile
+    environment:
+      - ROS_HOSTNAME=192.168.0.166
     volumes:
       - ./realm_gc:/catkin_ws/src/realm_gc # allows access to the ROS package in the realm directory
     network_mode: host
@@ -45,6 +51,8 @@ services:
     build:
       context: .
       dockerfile: .docker/Dockerfile
+    environment:
+      - ROS_HOSTNAME=192.168.0.166
     volumes:
       - ./realm_gc:/catkin_ws/src/realm_gc # allows access to the ROS package in the realm directory
     network_mode: host
@@ -61,6 +69,7 @@ services:
       context: .
       dockerfile: .docker/Dockerfile
     environment:
+      - ROS_HOSTNAME=192.168.0.166
       - DISPLAY=${DISPLAY} # allows GUI access
     volumes:
       - /tmp/.X11-unix:/tmp/.X11-unix # allows GUI access

realm_gc/rgc/launch/tro_all.launch

@@ -20,6 +20,7 @@
             <arg name="traj_filepath" default="base/non_ego_traj_0.eqx"/>
 
             <arg name="position_topic" default="/vicon/realm_turtle_1/realm_turtle_1"/>
+            <arg name="T" default="12"/>
         </include>
     </group>
 
@@ -29,6 +30,7 @@
             <arg name="traj_filepath" default="base/non_ego_traj_1.eqx"/>
 
             <arg name="position_topic" default="/vicon/realm_turtle_2/realm_turtle_2"/>
+            <arg name="T" default="12"/>
         </include>
     </group>
 </launch>

realm_gc/rgc/launch/tro_turtlebot.launch

@@ -3,6 +3,7 @@
 <launch>
     <arg name="base_path" default="$(find rgc_control)/saved_policies"/>
     <arg name="traj_filepath" default="base/non_ego_traj_0.eqx"/>
+    <arg name="T" default="6.0"/>
 
     <arg name="position_topic" default="/vicon/realm_turtle_1/realm_turtle_1"/>
 
@@ -14,12 +15,13 @@
     </node>
 
     <!-- Controller -->
-    <!-- <node name="turtlebot_controller" pkg="rgc_control" type="turtlebot_control.py" output="screen"> -->
+    <node name="turtlebot_controller" pkg="rgc_control" type="turtlebot_control.py" output="screen">
         <!-- Info on where to find the saved trajectory file -->
-        <!-- <param name="trajectory/base_path" type="string" value="$(arg base_path)"/> -->
-        <!-- <param name="trajectory/filename" type="string" value="$(arg traj_filepath)"/> -->
+        <param name="trajectory/base_path" type="string" value="$(arg base_path)"/>
+        <param name="trajectory/filename" type="string" value="$(arg traj_filepath)"/>
+        <param name="T" type="double" value="$(arg T)"/>
 
         <!-- Match up with state estimator topic name -->
-        <!-- <param name="state_estimate_topic" type="string" value="turtlebot_state_estimator"/> -->
-    <!-- </node> -->
+        <param name="state_estimate_topic" type="string" value="turtlebot_state_estimator/estimate"/>
+    </node>
 </launch>

realm_gc/rgc_control/src/rgc_control/policies/tracking/steering_policies.py

@@ -57,23 +57,29 @@ def compute_action(self, observation: SteeringObservation) -> TurtlebotAction:
         )
 
         # Compute the control action
-        linear_velocity = 0.5 * error[0]  # projection along the turtlebot x-axis
+        linear_velocity = 1.0 * error[0]  # projection along the turtlebot x-axis
 
         # Compute the angular velocity: steer towards the goal if we're far from it
         # (so the arctan is well defined), and align to the goal orientation
-        if np.linalg.norm(error) > 0.05:
+        if np.linalg.norm(error) > 0.1:
             angular_velocity = np.arctan2(error[1], error[0])
         else:
             angle_error = observation.goal.theta - observation.pose.theta
             if angle_error > np.pi:
                 angle_error -= 2 * np.pi
             if angle_error < -np.pi:
                 angle_error += 2 * np.pi
-            angular_velocity = 0.1 * angle_error
+            angular_velocity = 0.3 * angle_error
+
+        if isinstance(linear_velocity, np.ndarray):
+            linear_velocity = linear_velocity.item()
+
+        if isinstance(angular_velocity, np.ndarray):
+            angular_velocity = angular_velocity.item()        
 
         return TurtlebotAction(
-            linear_velocity=linear_velocity.item(),
-            angular_velocity=angular_velocity.item(),
+            linear_velocity=linear_velocity,
+            angular_velocity=angular_velocity,
         )
 
 

realm_gc/rgc_control/src/rgc_control/policies/tracking/tracking_policies.py

@@ -1,6 +1,9 @@
 """Define a policy for trajectory tracking."""
 from dataclasses import dataclass
 
+import jax
+import jax.numpy as jnp
+
 from rgc_control.policies.policy import ControlAction, ControlPolicy
 from rgc_control.policies.tracking.steering_policies import (
     Pose2DObservation,
@@ -43,16 +46,22 @@ def action_type(self):
 
     def compute_action(self, observation: TimedPose2DObservation) -> ControlAction:
         """Takes in an observation and returns a control action."""
-        # Compute the desired waypoint
+        # Compute the desired waypoint and tangent vector
         waypoint = self.trajectory(observation.t)
+        tangent = jax.jit(jax.jacfwd(self.trajectory))(observation.t)
+
+        # Compute the angle to steer along
+        theta = jnp.pi / 2
+        if jnp.linalg.norm(tangent) >= 0.05:
+            theta = jnp.arctan2(tangent[1], tangent[0])
 
         # Compute the control action to steer towards the waypoint
         steering_observation = SteeringObservation(
             pose=observation,
             goal=Pose2DObservation(
                 x=waypoint[0],
                 y=waypoint[1],
-                theta=0.0,
+                theta=theta,
                 v=0.0,
             ),
         )

realm_gc/rgc_control/src/rgc_control/policies/tro_experiment_policies.py

@@ -42,15 +42,16 @@ def create_tro_f1tenth_policy(
     """
     # Construct the components of the policy using the parameters they were trained with
 
-    # Load the trajectory and make it start at the right spot
+    # Start pointing along +y in the highbay
+    desired_equilibrium_state = jnp.array([0.0, 0.0, jnp.pi / 2.0, 1.0])
+
+    # Load the trajectory and flip x and y to convert from sim to high bay layout
     ego_traj = LinearTrajectory2D.from_eqx(6, traj_eqx_path)
-    ego_traj = LinearTrajectory2D(
-        p=ego_traj.p + initial_state[:2],
-    )
+    ego_traj = LinearTrajectory2D(p=jnp.fliplr(ego_traj.p))
 
     # Make the trajectory tracking policy
     steering_controller = F1TenthSteeringPolicy(
-        equilibrium_state=initial_state,
+        equilibrium_state=desired_equilibrium_state,
         axle_length=0.28,
         dt=0.1,
     )
@@ -78,9 +79,11 @@ def create_tro_turtlebot_policy(initial_position, traj_eqx_path) -> CompositePol
     """
     # Construct the components of the policy using the parameters they were trained with
 
-    # Load the trajectory and make it start at the right spot
+    # Load the trajectory and flip the x and y coordinates
     non_ego_traj = LinearTrajectory2D.from_eqx(2, traj_eqx_path)
-    non_ego_traj = LinearTrajectory2D(p=non_ego_traj.p + initial_position)
+    non_ego_traj = LinearTrajectory2D(p=jnp.fliplr(non_ego_traj.p))
+    print("Loaded trajectory with waypoints:")
+    print(non_ego_traj.p)
 
     # Make the trajectory tracking policy
     steering_controller = TurtlebotSteeringPolicy()

realm_gc/rgc_control/src/rgc_control/robot_control.py

@@ -22,8 +22,8 @@ def __init__(self):
         # Control publisher - to be defined in subclasses, as needed
         self.control_pub = None
 
-        # Initialize the control
-        self.ctrl_c = False
+        # Initialize the control as stopped
+        self.ctrl_c = True
 
         # Create a subscriber for starting/stopping the controller
         self.start_sub = rospy.Subscriber(

realm_gc/rgc_control/src/rgc_control/turtlebot_control.py

@@ -19,13 +19,13 @@ def __init__(self):
         super(TurtlebotControl, self).__init__()
 
         # Publish cmd velocity for state estimation
-        self.control_pub = rospy.Publisher("/cmd", Twist, queue_size=1)
+        self.control_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
         self.control = TurtlebotAction(0.0, 0.0)
 
         # Subscribe to State estimation topic from ros param
         self.state = None
         self.state_estimate_topic = rospy.get_param(
-            "~state_estimate_topic", f"{rospy.get_name()}/estimate"
+            "~state_estimate_topic", f"turtlebot_state_estimator/estimate"
         )
         self.estimate_sub = rospy.Subscriber(
             self.state_estimate_topic, TurtlebotState, self.state_estimate_callback