From d878d3f495b6ec40fa8570dbf2a583af3ef91cad Mon Sep 17 00:00:00 2001
From: Ethan Gordon <ekgordon@cs.washington.edu>
Date: Mon, 14 Sep 2020 17:35:44 -0700
Subject: [PATCH] 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.
---
 include/libada/detail/Ada-impl.hpp | 17 +++++++++++------
 src/Ada.cpp                        |  4 ++--
 2 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/include/libada/detail/Ada-impl.hpp b/include/libada/detail/Ada-impl.hpp
index 1eda129..22c4cf2 100644
--- 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
index f96648d..a64c47e 100644
--- 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();
 }
 
 //==============================================================================