fix: minor fixes

Author: Meow404

examples/plate-balancing/config/C3/controller-options/2_supports.yaml

@@ -5,11 +5,11 @@ solve_time_filter_alpha: 0.95
 publish_frequency: 0
 state_prediction_joints:
   - name: "plate_x"
-    max_acceleration: 10
+    max_acceleration: 2
   - name: "plate_y"
-    max_acceleration: 10
+    max_acceleration: 2
   - name: "plate_z"
-    max_acceleration: 10
+    max_acceleration: 2
 
 # Workspace Limits
 # End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub]

examples/plate-balancing/config/Simulation/scene-config/4_wall.yaml

@@ -6,5 +6,5 @@ camera_target: [ 0.5, 0, 0.5 ]
 environment_models: [examples/plate-balancing/urdf/side_wall.urdf]
 # orientation in RPY then position in XYZ
 environment_orientations: [[0.0, 0.0, 0.0]]
-environment_positions: [[0.6, 0.3, 0.35]]
+environment_positions: [[0.6, 0.285, 0.35]]
 

examples/plate-balancing/systems/end_effector_orientation.cc

@@ -52,8 +52,6 @@ void EndEffectorOrientationTrajectoryGenerator::CalcTraj(
     *casted_traj = *trajectory_input;
   } else {
     PiecewiseQuaternionSlerp<double> result;
-    Eigen::VectorXd neutral_quaternion = VectorXd::Zero(4);
-    neutral_quaternion(0) = 1;
     result = drake::trajectories::PiecewiseQuaternionSlerp<double>(
         {0, 1},
         {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)});

examples/plate-balancing/urdf/side_wall.urdf

@@ -15,7 +15,7 @@
         <collision name="wall">
             <origin rpy="0 0 0" xyz="0 0 0.0"/>
              <geometry>
-                <box size="0.5 0.05 0.7"/>cod
+                <box size="0.5 0.05 0.7"/>
             </geometry>
             <drake:proximity_properties>
                 <drake:rigid_hydroelastic/>
@@ -26,7 +26,7 @@
         <visual>
             <origin rpy="0 0 0" xyz="0 0 0.0"/>
             <geometry>
-                <box size="0.5 0.05 0.7"/>cod
+                <box size="0.5 0.05 0.7"/>
             </geometry>
             <material name="silver">
               <color rgba="0.75 0.75 0.75 1"/>

examples/plate-balancing/urdf/tray_lcs.sdf

@@ -36,7 +36,6 @@
             <length>0.022</length>
           </cylinder>
         </geometry>
-        <pose>0.15 0.3 0 0 0 0</pose>
       </collision>
     </link>
   </model>

lcmtypes/BUILD.bazel

@@ -56,6 +56,7 @@ drake_java_binary(
     visibility = ["//visibility:private"],
     runtime_deps = [
         ":lcmtypes_robot_java",
+        "@c3//lcmtypes:lcmtypes_c3_java",
         "@drake//lcmtypes:lcmtypes_drake_java",
     ],
 )

systems/controllers/osc/external_force_tracking_data.cc

@@ -37,7 +37,7 @@ void ExternalForceTrackingData::Update(
     const drake::systems::Context<double>& context_wo_spr,
     const drake::trajectories::Trajectory<double>& traj, double t) {
   DRAKE_DEMAND(traj.rows() == 3);
-  lambda_des_ = traj.value(t);
+  lambda_des_ = -traj.value(t);
 }
 
 }  // namespace dairlib::systems::controllers

systems/controllers/osc/operational_space_control.cc

@@ -403,7 +403,11 @@ VectorXd OperationalSpaceControl::SolveQp(
   const auto active_contact_names = contact_names_map_.count(fsm_state) > 0
                                         ? contact_names_map_.at(fsm_state)
                                         : std::vector<std::string>();
-  id_qp_.UpdateDynamics(x_w_spr, active_contact_names, {});
+  std::vector<std::string> active_external_forces = {};
+  for (const auto& force_tracking_data : *force_tracking_data_vec_) {
+    active_external_forces.push_back(force_tracking_data->GetName());
+  }
+  id_qp_.UpdateDynamics(x_w_spr, active_contact_names, active_external_forces);
 
   //  Invariant Impacts
   //  Only update when near an impact

systems/trajectory_optimization/lcm_trajectory_systems.cc

@@ -216,8 +216,9 @@ void LcmC3TrajectoryReceiver::OutputQuaternionTrajectory(
     } else if (trajectory_block.datapoints.rows() == 4) {
       Eigen::Vector4d quat = trajectory_block.datapoints.topRows(4).col(i);
       quaternion_datapoints.push_back(
-          !quat.isZero() ? Eigen::Quaternion<double>(quat)
-                         : Eigen::Quaternion<double>(1, 0, 0, 0));
+          !quat.isZero()
+              ? Eigen::Quaternion<double>(quat[0], quat[1], quat[2], quat[3])
+              : Eigen::Quaternion<double>(1, 0, 0, 0));
     } else {
       throw std::runtime_error(
           "Invalid trajectory data points size: " +