Clean-up + format

moveit · Oct 4, 2024 · 421f34e · 421f34e
1 parent 31265e8
commit 421f34e
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 5 deletions.
diff --git a/src/add_toppra_time_parameterization.cpp b/src/add_toppra_time_parameterization.cpp
@@ -156,8 +156,10 @@ class AddToppraTimeParameterization : public planning_interface::PlanningRespons
     auto& plant = diagram_->GetDowncastSubsystemByName<MultibodyPlant<double>>("plant");
     auto& plant_context = diagram_->GetMutableSubsystemContext(plant, diagram_context_.get());
     Eigen::VectorXd q = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities());
-    Eigen::VectorXd joint_positions = moveit::drake::getJointPositionVector(res.trajectory->getFirstWayPoint(), joint_model_group->getName(), plant);
-    Eigen::VectorXd joint_velocities = moveit::drake::getJointVelocityVector(res.trajectory->getFirstWayPoint(), joint_model_group->getName(), plant);
+    Eigen::VectorXd joint_positions =
+        moveit::drake::getJointPositionVector(res.trajectory->getFirstWayPoint(), joint_model_group->getName(), plant);
+    Eigen::VectorXd joint_velocities =
+        moveit::drake::getJointVelocityVector(res.trajectory->getFirstWayPoint(), joint_model_group->getName(), plant);
     q << joint_positions, joint_velocities;
     plant.SetPositionsAndVelocities(&plant_context, q);
 

diff --git a/src/conversions.cpp b/src/conversions.cpp
@@ -57,7 +57,6 @@ using ::drake::systems::DiagramBuilder;
     const auto& joint_index = plant.GetJointByName(joint_name).ordinal();
     joint_positions(joint_index) = moveit_state.getVariablePosition(joint_name);
   }
-  std::cout << "Joint positions: " << joint_positions.transpose() << std::endl;
   return joint_positions;
 }
 
@@ -73,7 +72,6 @@ using ::drake::systems::DiagramBuilder;
     const auto& joint_index = plant.GetJointByName(joint_name).ordinal();
     joint_velocities(joint_index) = moveit_state.getVariableVelocity(joint_name);
   }
-  std::cout << "Joint velocities: " << joint_velocities.transpose() << std::endl;
   return joint_velocities;
 }
 
@@ -165,7 +163,6 @@ void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& drake_t
     const auto t_scale = static_cast<double>(i) / static_cast<double>(num_pts - 1);
     const auto t = std::min(t_scale, 1.0) * drake_trajectory.end_time();
     const auto pos_val = drake_trajectory.value(t);
-    std::cout << "pos_val size: " << pos_val.size() << std::endl;
     const auto vel_val = drake_trajectory.EvalDerivative(t);
     const auto waypoint = std::make_shared<moveit::core::RobotState>(moveit_trajectory->getRobotModel());
     for (const auto& joint_model : active_joints)