switch dynamics to velocity

nocturne/cpp/include/cyclist.h

@@ -14,20 +14,20 @@ class Cyclist : public Object {
   Cyclist() = default;
 
   Cyclist(int64_t id, float length, float width,
-          const geometry::Vector2D& position, float heading, float speed,
+          const geometry::Vector2D& position, float heading, geometry::Vector2D velocity,
           const geometry::Vector2D& target_position, float target_heading,
           float target_speed, bool can_block_sight = true,
           bool can_be_collided = true, bool check_collision = true)
-      : Object(id, length, width, position, heading, speed, target_position,
+      : Object(id, length, width, position, heading, velocity, target_position,
                target_heading, target_speed, can_block_sight, can_be_collided,
                check_collision) {}
 
   Cyclist(int64_t id, float length, float width, float max_speed,
-          const geometry::Vector2D& position, float heading, float speed,
+          const geometry::Vector2D& position, float heading, geometry::Vector2D velocity,
           const geometry::Vector2D& target_position, float target_heading,
           float target_speed, bool can_block_sight = true,
           bool can_be_collided = true, bool check_collision = true)
-      : Object(id, length, width, max_speed, position, heading, speed,
+      : Object(id, length, width, max_speed, position, heading, velocity,
                target_position, target_heading, target_speed, can_block_sight,
                can_be_collided, check_collision) {}
 

nocturne/cpp/include/object.h

@@ -35,7 +35,7 @@ class Object : public ObjectBase {
   Object() = default;
 
   Object(int64_t id, float length, float width,
-         const geometry::Vector2D& position, float heading, float speed,
+         const geometry::Vector2D& position, float heading, const geometry::Vector2D& velocity,
          const geometry::Vector2D& target_position, float target_heading,
          float target_speed, bool can_block_sight = true,
          bool can_be_collided = true, bool check_collision = true)
@@ -44,7 +44,7 @@ class Object : public ObjectBase {
         length_(length),
         width_(width),
         heading_(heading),
-        speed_(ClipSpeed(speed)),
+        velocity_(ClipSpeed(velocity)),
         target_position_(target_position),
         target_heading_(target_heading),
         target_speed_(target_speed),
@@ -53,7 +53,7 @@ class Object : public ObjectBase {
   }
 
   Object(int64_t id, float length, float width, float max_speed,
-         const geometry::Vector2D& position, float heading, float speed,
+         const geometry::Vector2D& position, float heading, const geometry::Vector2D& velocity,
          const geometry::Vector2D& target_position, float target_heading,
          float target_speed, bool can_block_sight = true,
          bool can_be_collided = true, bool check_collision = true)
@@ -63,7 +63,7 @@ class Object : public ObjectBase {
         width_(width),
         max_speed_(max_speed),
         heading_(heading),
-        speed_(ClipSpeed(speed)),
+        velocity_(ClipSpeed(velocity)),
         target_position_(target_position),
         target_heading_(target_heading),
         target_speed_(target_speed),
@@ -82,8 +82,9 @@ class Object : public ObjectBase {
   float heading() const { return heading_; }
   void set_heading(float heading) { heading_ = heading; }
 
-  float speed() const { return speed_; }
-  void set_speed(float speed) { speed_ = ClipSpeed(speed); }
+  float speed() const { return velocity_.Norm(); }
+  const geometry::Vector2D& velocity() const { return velocity_; }
+  void set_velocity(geometry::Vector2D velocity) { velocity_ = ClipSpeed(velocity_); }
 
   const geometry::Vector2D& target_position() const { return target_position_; }
   void set_target_position(const geometry::Vector2D& target_position) {
@@ -142,7 +143,7 @@ class Object : public ObjectBase {
   }
 
   geometry::Vector2D Velocity() const {
-    return geometry::PolarToVector2D(speed_, heading_);
+    return velocity_;
   }
 
   geometry::ConvexPolygon BoundingPolygon() const override;
@@ -180,8 +181,13 @@ class Object : public ObjectBase {
 
   void KinematicBicycleStep(float dt);
 
-  float ClipSpeed(float speed) const {
-    return std::max(std::min(speed, max_speed_), -max_speed_);
+  geometry::Vector2D ClipSpeed(geometry::Vector2D velocity) const {
+    // project the velocity onto the circle of radius max_speed_
+    const float speed = velocity.Norm();
+    if (std::abs(speed) < max_speed_) {
+      return velocity;
+    }
+    return velocity * (max_speed_ / speed);
   }
 
   const int64_t id_;
@@ -191,8 +197,7 @@ class Object : public ObjectBase {
   const float max_speed_ = std::numeric_limits<float>::max();
 
   float heading_ = 0.0f;
-  // Postive for moving forward, negative for moving backward.
-  float speed_ = 0.0f;
+  geometry::Vector2D velocity_ = geometry::Vector2D(0.0f, 0.0f);
 
   geometry::Vector2D target_position_;
   float target_heading_ = 0.0f;

nocturne/cpp/include/pedestrian.h

@@ -14,20 +14,20 @@ class Pedestrian : public Object {
   Pedestrian() = default;
 
   Pedestrian(int64_t id, float length, float width,
-             const geometry::Vector2D& position, float heading, float speed,
+             const geometry::Vector2D& position, float heading, geometry::Vector2D velocity,
              const geometry::Vector2D& target_position, float target_heading,
              float target_speed, bool can_block_sight = true,
              bool can_be_collided = true, bool check_collision = true)
-      : Object(id, length, width, position, heading, speed, target_position,
+      : Object(id, length, width, position, heading, velocity, target_position,
                target_heading, target_speed, can_block_sight, can_be_collided,
                check_collision) {}
 
   Pedestrian(int64_t id, float length, float width, float max_speed,
-             const geometry::Vector2D& position, float heading, float speed,
+             const geometry::Vector2D& position, float heading, geometry::Vector2D velocity,
              const geometry::Vector2D& target_position, float target_heading,
              float target_speed, bool can_block_sight = true,
              bool can_be_collided = true, bool check_collision = true)
-      : Object(id, length, width, max_speed, position, heading, speed,
+      : Object(id, length, width, max_speed, position, heading, velocity,
                target_position, target_heading, target_speed, can_block_sight,
                can_be_collided, check_collision) {}
 

nocturne/cpp/include/vehicle.h

@@ -14,21 +14,21 @@ class Vehicle : public Object {
   Vehicle() = default;
 
   Vehicle(int64_t id, float length, float width,
-          const geometry::Vector2D& position, float heading, float speed,
+          const geometry::Vector2D& position, float heading, geometry::Vector2D velocity,
           const geometry::Vector2D& target_position, float target_heading,
           float target_speed, bool is_av, bool can_block_sight = true,
           bool can_be_collided = true, bool check_collision = true)
-      : Object(id, length, width, position, heading, speed, target_position,
+      : Object(id, length, width, position, heading, velocity, target_position,
                target_heading, target_speed, can_block_sight, can_be_collided,
                check_collision),
                is_av_(is_av){}
 
   Vehicle(int64_t id, float length, float width, float max_speed,
-          const geometry::Vector2D& position, float heading, float speed,
+          const geometry::Vector2D& position, float heading, geometry::Vector2D velocity,
           const geometry::Vector2D& target_position, float target_heading,
           float target_speed, bool is_av, bool can_block_sight = true,
           bool can_be_collided = true, bool check_collision = true)
-      : Object(id, length, width, max_speed, position, heading, speed,
+      : Object(id, length, width, max_speed, position, heading, velocity,
                target_position, target_heading, target_speed, can_block_sight,
                can_be_collided, check_collision),
                 is_av_(is_av)

nocturne/cpp/src/object.cc

@@ -87,13 +87,13 @@ void Object::SetActionFromKeyboard() {
     acceleration_ = 1.0f;
   } else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Down)) {
     // larger acceleration for braking than for moving backwards
-    acceleration_ = speed_ > 0 ? -2.0f : -1.0f;
-  } else if (std::abs(speed_) < 0.05) {
+    acceleration_ = velocity_.Norm() > 0 ? -2.0f : -1.0f;
+  } else if (std::abs(velocity_.Norm()) < 0.05) {
     // clip to 0
-    speed_ = 0.0f;
+    velocity_ = geometry::Vector2D(0.0f, 0.0f);
   } else {
     // friction
-    acceleration_ = 0.5f * (speed_ > 0 ? -1.0f : 1.0f);
+    acceleration_ = 0.5f * (velocity_.Norm() > 0 ? -1.0f : 1.0f);
   }
 
   // right: turn right; left: turn left
@@ -115,11 +115,11 @@ void Object::KinematicBicycleStep(float dt) {
   //     new_yaw = yaw + steering * (speed * t + 1/2 * accel * t ** 2)
   //     new_vel = vel + accel * t
   geometry::Vector2D vel = Velocity();
-  geometry::Vector2D rot = geometry::Vector2D(std::cos(heading_), std::sin(heading_));
-  position_ = position_ + vel * dt + 0.5 * acceleration_ * rot * (float)pow(dt, 2);
+  position_ = position_ + vel * dt + 0.5 * acceleration_ * (vel / vel.Norm()) * (float)pow(dt, 2);
 
-  heading_ = geometry::utils::AngleAdd(heading_, (float)(steering_ * (speed_ * dt + 0.5 * acceleration_ * pow(dt, 2))));
-  speed_ = ClipSpeed(speed_ + acceleration_ * dt);
+  heading_ = geometry::utils::AngleAdd(heading_, (float)(steering_ * (vel.Norm() * dt + 0.5 * acceleration_ * pow(dt, 2))));
+  float new_vel = vel.Norm() + acceleration_ * dt;
+  velocity_ = ClipSpeed(geometry::Vector2D(new_vel * cos(heading_), new_vel * sin(heading_)));
 }
 
 }  // namespace nocturne

nocturne/cpp/src/scenario.cc

@@ -273,14 +273,14 @@ void Scenario::Step(float dt) {
       const int64_t obj_id = object->id();
       object->set_position(expert_trajectories_.at(obj_id).at(current_time_));
       object->set_heading(expert_headings_.at(obj_id).at(current_time_));
-      object->set_speed(expert_speeds_.at(obj_id).at(current_time_));
+      object->set_velocity(expert_velocities_.at(obj_id).at(current_time_));
     }
   }
   for (auto& object : traffic_lights_) {
     object->set_current_time(current_time_);
   }
-  std::cout << "[C++] t = " << current_time_ << std::endl;
-  std::cout << "[C++] expert heading" << expert_headings_.at(22).at(current_time_) << std::endl;
+  // std::cout << "[C++] t = " << current_time_ << std::endl;
+  // std::cout << "[C++] expert heading" << expert_headings_.at(22).at(current_time_) << std::endl;
 
   // update the vehicle bvh
   object_bvh_.Reset(objects_);
@@ -562,20 +562,22 @@ std::optional<Action> Scenario::ExpertAction(const Object& obj,
   // a_t = (v_{t+1} - v_t) / dt
   const float acceleration =
       (cur_speeds[timestamp + 1] - cur_speeds[timestamp]) / expert_dt_;
+
+  float new_yaw = cur_headings[timestamp + 1];
   // Calculate the updated yaw using the velocity angle
   float real_new_yaw = std::atan2(cur_velocities[timestamp + 1].y(),
                                   cur_velocities[timestamp + 1].x());
   // TODO: @ev Update real_new_yaw
-  // if (cur_speeds[timestamp + 1] < 0.6) {
-  //   real_new_yaw = new_yaw;
-  // }
+  if (cur_speeds[timestamp + 1] < 0.6) {
+    real_new_yaw = new_yaw;
+  }
   // Compute delta yaw
   const float delta_yaw = geometry::utils::AngleSub(real_new_yaw,
                                                     cur_headings[timestamp]);
   // Calculate steering.
   float steering = delta_yaw / (cur_speeds[timestamp] * expert_dt_ +
                                 0.5 * acceleration * pow(expert_dt_, 2));
-  if (cur_speeds[timestamp] < 0.6) {
+  if (cur_speeds[timestamp] < 0.6 || cur_speeds[timestamp + 1] < 0.6 ) {
     steering = 0.0;
   }
   // return action
@@ -965,7 +967,7 @@ void Scenario::LoadObjects(const json& objects_json) {
     if (object_type == ObjectType::kVehicle) {
       std::shared_ptr<Vehicle> vehicle = std::make_shared<Vehicle>(
           cur_id, length, width, position, cur_headings[current_time_],
-          cur_speeds[current_time_], target_position, target_heading,
+          cur_velocities[current_time_], target_position, target_heading,
           target_speed, is_av);
       vehicles_.push_back(vehicle);
       objects_.push_back(vehicle);
@@ -976,7 +978,7 @@ void Scenario::LoadObjects(const json& objects_json) {
       if (object_type == ObjectType::kPedestrian) {
         std::shared_ptr<Pedestrian> pedestrian = std::make_shared<Pedestrian>(
             cur_id, length, width, position, cur_headings[current_time_],
-            cur_speeds[current_time_], target_position, target_heading,
+            cur_velocities[current_time_], target_position, target_heading,
             target_speed);
         pedestrians_.push_back(pedestrian);
         objects_.push_back(pedestrian);
@@ -986,7 +988,7 @@ void Scenario::LoadObjects(const json& objects_json) {
       } else if (object_type == ObjectType::kCyclist) {
         std::shared_ptr<Cyclist> cyclist = std::make_shared<Cyclist>(
             cur_id, length, width, position, cur_headings[current_time_],
-            cur_speeds[current_time_], target_position, target_heading,
+            cur_velocities[current_time_], target_position, target_heading,
             target_speed);
         cyclists_.push_back(cyclist);
         objects_.push_back(cyclist);

nocturne/cpp/tests/src/object_test.cc

@@ -44,7 +44,7 @@ TEST(ObjectTest, UniformLinearMotionTest) {
   const float target_heading = heading;
   const float target_speed = speed;
 
-  Object obj(/*id=*/0, length, width, position, heading, speed, target_position,
+  Object obj(/*id=*/0, length, width, position, heading, velocity, target_position,
              target_heading, target_speed);
   const int num_steps = 100;
   const float dt = t / static_cast<float>(num_steps);
@@ -73,7 +73,7 @@ TEST(ObjectTest, ConstantAccelerationMotionTest) {
   float target_speed = speed + acceleration * t;
 
   // Forward test.
-  Object obj(/*id=*/0, length, width, position, heading, speed, target_position,
+  Object obj(/*id=*/0, length, width, position, heading, velocity, target_position,
              target_heading, target_speed);
   obj.set_acceleration(acceleration);
   const int num_steps = 100;
@@ -95,7 +95,7 @@ TEST(ObjectTest, ConstantAccelerationMotionTest) {
       geometry::PolarToVector2D(acceleration, heading) * (t * t * 0.5f);
   target_speed = speed + acceleration * t;
   obj.set_position(position);
-  obj.set_speed(speed);
+  obj.set_velocity(velocity);
   obj.set_target_position(target_position);
   obj.set_target_speed(target_speed);
   obj.set_acceleration(acceleration);
@@ -130,7 +130,7 @@ TEST(ObjectTest, SpeedCliptTest) {
   float target_speed = max_speed;
 
   // Forward test.
-  Object obj(/*id=*/0, length, width, max_speed, position, heading, speed,
+  Object obj(/*id=*/0, length, width, max_speed, position, heading, velocity,
              target_position, target_heading, target_speed);
   obj.set_acceleration(acceleration);
   const int num_steps = 100;
@@ -152,7 +152,7 @@ TEST(ObjectTest, SpeedCliptTest) {
       final_velocity * t2;
   target_speed = -max_speed;
   obj.set_position(position);
-  obj.set_speed(speed);
+  obj.set_velocity(velocity);
   obj.set_target_position(target_position);
   obj.set_target_speed(target_speed);
   obj.set_acceleration(acceleration);
@@ -170,14 +170,15 @@ TEST(ObjectTest, SteeringMotionTest) {
   const float width = 1.0f;
   const float heading = kQuarterPi;
   const float speed = 2.0f;
+  geometry::Vector2D velocity = geometry::PolarToVector2D(speed, heading);
   const float steering = geometry::utils::Radians(10.0f);
   const float dt = 0.1f;
   const geometry::Vector2D position(1.0f, 1.0f);
   const auto [target_position, target_heading] =
       KinematicBicycleModel(position, length, heading, speed, steering, dt);
   const float target_speed = speed;
 
-  Object obj(/*id=*/0, length, width, position, heading, speed, target_position,
+  Object obj(/*id=*/0, length, width, position, heading, velocity, target_position,
              target_heading, target_speed);
   obj.set_steering(steering);
   obj.Step(dt);

nocturne/envs/base_env.py

@@ -156,14 +156,15 @@ def step(  # pylint: disable=arguments-renamed,too-many-locals,too-many-branches
 
         my_av_obj = self.controlled_vehicles[0]
         logging.debug(f"--- t = {self.step_num} ---")
-        logging.debug(f"applied_actions: {action_dict[my_av_obj.id]} \n")
+        if len(action_dict) > 0:
+            logging.debug(f"applied_actions: {action_dict[my_av_obj.id]} \n")
 
         logging.debug(f"veh_pos_____: ({my_av_obj.position.x:.3f}, {my_av_obj.position.y:.3f})")
         logging.debug(
             f"true_veh_pos: ({self.scenario.expert_position(my_av_obj, self.step_num).x:.3f}, {self.scenario.expert_position(my_av_obj, self.step_num).y:.3f}) \n"
         )
 
-        logging.debug(f"veh_speed_____: {my_av_obj.speed:.3f}")
+        logging.debug(f"veh_speed_____: {my_av_obj.speed():.3f}")
         logging.debug(f"true_veh_speed: {self.scenario.expert_speed(my_av_obj, self.step_num):.3f} \n")
 
         logging.debug(f"veh_heading_____: {my_av_obj.heading:.3f}")
@@ -215,7 +216,7 @@ def step(  # pylint: disable=arguments-renamed,too-many-locals,too-many-branches
             if rew_cfg.position_target:
                 position_target_achieved = (goal_pos - obj_pos).norm() < rew_cfg.position_target_tolerance
             if rew_cfg.speed_target:
-                speed_target_achieved = np.abs(veh_obj.speed - veh_obj.target_speed) < rew_cfg.speed_target_tolerance
+                speed_target_achieved = np.abs(veh_obj.speed() - veh_obj.target_speed) < rew_cfg.speed_target_tolerance
             if rew_cfg.heading_target:
                 heading_target_achieved = (
                     np.abs(_angle_sub(veh_obj.heading, veh_obj.target_heading)) < rew_cfg.heading_target_tolerance
@@ -248,13 +249,13 @@ def step(  # pylint: disable=arguments-renamed,too-many-locals,too-many-branches
                     if rew_cfg.goal_distance_penalty:
                         rew_dict[veh_id] -= (
                             rew_cfg.shaped_goal_distance_scaling
-                            * (np.abs(veh_obj.speed - veh_obj.target_speed) / rew_cfg.goal_speed_scaling)
+                            * (np.abs(veh_obj.speed() - veh_obj.target_speed) / rew_cfg.goal_speed_scaling)
                             / rew_cfg.reward_scaling
                         )
                     else:
                         rew_dict[veh_id] += (
                             rew_cfg.shaped_goal_distance_scaling
-                            * (1 - np.abs(veh_obj.speed - veh_obj.target_speed) / rew_cfg.goal_speed_scaling)
+                            * (1 - np.abs(veh_obj.speed() - veh_obj.target_speed) / rew_cfg.goal_speed_scaling)
                             / rew_cfg.reward_scaling
                         )
                 if rew_cfg.shaped_goal_distance and rew_cfg.heading_target:
@@ -371,7 +372,7 @@ def reset(  # pylint: disable=arguments-differ,too-many-locals,too-many-branches
                     f"INIT_true_veh_pos: ({self.scenario.expert_position(my_av_obj, self.step_num).x:.4f}, {self.scenario.expert_position(my_av_obj, self.step_num).y:.4f}) \n"
                 )
 
-                logging.debug(f"INIT_veh_speed_____: {my_av_obj.speed:.3f}")
+                logging.debug(f"INIT_veh_speed_____: {my_av_obj.speed():.3f}")
                 logging.debug(f"INIT_true_veh_speed: {self.scenario.expert_speed(my_av_obj, self.step_num):.3f} \n")
 
                 logging.debug(f"INIT_veh_heading_____: {my_av_obj.heading:.3f}")