Make sure acceleration limits match velocity limits (#46)

* Make sure acceleration limits match velocity limits * Switched vel/acc limit size check to match arm DoF (excluding hand) * Removed redundant size check * Modify getAccelerationLimits and getVelocityLimits to only return arm joints.
personalrobotics · Sep 15, 2020 · d878d3f · d878d3f
1 parent 9e0e014
commit d878d3f
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 8 deletions.
diff --git a/include/libada/detail/Ada-impl.hpp b/include/libada/detail/Ada-impl.hpp
@@ -10,12 +10,17 @@ aikido::trajectory::UniqueSplinePtr Ada::postProcessPath(
     const Eigen::VectorXd& velocityLimits,
     const Eigen::VectorXd& accelerationLimits)
 {
-  auto sentVelocityLimits = (velocityLimits.squaredNorm() == 0.0)
-                                ? getVelocityLimits()
-                                : velocityLimits;
-  auto sentAccelerationLimits = (accelerationLimits.squaredNorm() == 0.0)
-                                    ? getAccelerationLimits()
-                                    : accelerationLimits;
+  bool velLimitsInvalid
+      = (velocityLimits.squaredNorm() == 0.0)
+        || velocityLimits.size() != getVelocityLimits().size();
+  auto sentVelocityLimits
+      = velLimitsInvalid ? getVelocityLimits() : velocityLimits;
+
+  bool accLimitsInvalid
+      = (accelerationLimits.squaredNorm() == 0.0)
+        || accelerationLimits.size() != getAccelerationLimits().size();
+  auto sentAccelerationLimits
+      = accLimitsInvalid ? getAccelerationLimits() : accelerationLimits;
 
   return mRobot->postProcessPath<PostProcessor>(
       sentVelocityLimits,

diff --git a/src/Ada.cpp b/src/Ada.cpp
@@ -569,13 +569,13 @@ Eigen::VectorXd Ada::getCurrentConfiguration() const
 //==============================================================================
 Eigen::VectorXd Ada::getVelocityLimits() const
 {
-  return mRobot->getMetaSkeleton()->getVelocityUpperLimits();
+  return mArm->getMetaSkeleton()->getVelocityUpperLimits();
 }
 
 //==============================================================================
 Eigen::VectorXd Ada::getAccelerationLimits() const
 {
-  return mRobot->getMetaSkeleton()->getAccelerationUpperLimits();
+  return mArm->getMetaSkeleton()->getAccelerationUpperLimits();
 }
 
 //==============================================================================