Added SNS Acceleration IK Solver Interface with UnitTest by robodreamer · Pull Request #102 · RethinkRobotics-opensource/sns_ik

robodreamer · 2018-09-27T20:43:06Z

added sns acceleration ik class and SNS_Base_Acc type
added Jacobian dot computation
created unit test with sawyer model
fixed some bugs in sns_ik_acc_base and sns_ik_vel_base codes

NOTE: Acc IK solver was created as another type of Vel IK solver,
I am trying to figure out a better way to do this, so any suggestions
are more than welcome! But this code works and it passes unit test.

- added sns acceleration ik class and SNS_Base_Acc type - added Jacobian dot computation - created unit test with sawyer model

chris-smith

Not sure I know the code well enough to provide useful suggestions RE your note...

chris-smith · 2018-09-28T17:10:56Z

sns_ik_lib/src/sns_acceleration_ik.cpp

+
+  if (n_tasks != ntasks) {
+    n_tasks = ntasks;
+    Eigen::VectorXd ddq = Eigen::VectorXd::Zero(n_dof);


unnecessary? Or is this supposed to set a member variable?

It is a member variable. I followed the original code (SNS velocity solvers)'s style, so it is hard to tell whether it is a local variable or not. We should revisit all these code once and revise the style..

chris-smith · 2018-09-28T17:16:04Z

sns_ik_lib/src/sns_acceleration_ik.cpp

+    // for the minimum bound
+    max = -maxJointAcceleration(i);
+    if (m_usePositionLimits) {
+      step = 2 * (jointLimit_low(i) - actualJointConfiguration(i) - actualJointVelocities(i)*loop_period) / (loop_period * loop_period);


is this taken from something specific I'm not aware of? worth a comment?

It is based on the equations in the original paper. I will add a comment.

chris-smith · 2018-09-28T17:20:43Z

sns_ik_lib/src/sns_ik.cpp

+                         const KDL::JntArray& q_bias,
+                         const std::vector<std::string>& biasNames,
+                         const KDL::JntArray& q_acc_bias,
+                         KDL::JntArray& qddot_out)


should output variable be a pointer? That looks like the convention

Yes. I followed the original code style, which is not necessarily good but it seemed to require a large amount of additional code changes once changed.

forrest-rm

Overall, it looks mostly good.

forrest-rm · 2018-10-01T13:26:44Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+    // The control loop period in seconds
+    void setLoopPeriod(double period) { loop_period = period; }
+
+    // SNS Acceleration IK


This function needs more documentation for inputs and outputs

Overall, more documentation is still needed.

forrest-rm · 2018-10-01T13:27:08Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+
+    // SNS Acceleration IK
+    int getJointAcceleration(Eigen::VectorXd *jointAcceleration, const std::vector<TaskAcc> &sot,
+                                    const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities);


Check alignment with previous line

forrest-rm · 2018-10-01T13:28:13Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+    int getJointAcceleration(Eigen::VectorXd *jointAcceleration, const std::vector<TaskAcc> &sot,
+                                    const Eigen::VectorXd &jointConfiguration, const Eigen::VectorXd &jointVelocities);
+
+    std::vector<double> getTasksScaleFactor()


The getters should be const functions:
getTaskScaleFactor() const {

Good point. Fixed it.

forrest-rm · 2018-10-01T13:38:10Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+    Eigen::ArrayXd ddotQmax;  // higher joint velocity bound
+    std::vector<double> scaleFactors;
+
+    // variables for base acc ik solver


It looks like most of these variables are only used in one function. They should not be member variables.

That is right. I moved them inside the function now.

forrest-rm · 2018-10-01T13:40:14Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+
+    SnsAccIkBase::uPtr baseIkSolver;
+
+    SnsIkBase::ExitCode exitCode;


forrest-rm · 2018-10-01T14:44:34Z

sns_ik_lib/src/sns_ik_base.cpp

@@ -189,9 +189,12 @@ SnsIkBase::ExitCode SnsIkBase::computeTaskScalingFactor(const Eigen::MatrixXd& J
  Eigen::ArrayXd jntScaleFactorArr(nJnt_);


What does "Arr" stand for?

It just indicates that the variable type is an Eigen array instead of a vector. It is the convention that was used in Matt's initial sns ik code.

forrest-rm · 2018-10-01T14:45:19Z

sns_ik_lib/src/sns_ik_base.cpp

    if (jntIsFree[i]) {
      jntScaleFactorArr(i) = SnsIkBase::findScaleFactor(lowMargin(i), uppMargin(i), a(i));
+      if (jntScaleFactorArr(i) == 1 && (jointOut(i) < (qLow_(i) - SnsIkBase::BOUND_TOLERANCE) || jointOut(i) > (qUpp_(i) + SnsIkBase::BOUND_TOLERANCE)))
+        jntScaleFactorArr(i) = 1e-3;


Magic number

forrest-rm · 2018-10-01T15:01:42Z

sns_ik_lib/test/sns_ik_acc_test.cpp

+      results.solveTime += tmp.solveTime;
+      results.fkLinErr += tmp.fkLinErr;
+      results.fkAngErr += tmp.fkAngErr;
+      ASSERT_LE(tmp.fkLinErr, ACC_IK_TEST_FK_LIN_TOL);


Should these asserts also happen when iTest == 0? If so, move them out of the else statement.

forrest-rm · 2018-10-01T15:02:45Z

sns_ik_lib/test/sns_ik_acc_test.cpp

+  // Run the test:
+  /*
+   * Note: This seems to cause a segfault in ubuntu 16.04 with Eigen 3.3.4, but it
+   * works fine in Ubuntu 14.04 with Eigen 3.2.0. FIXME


Is there a issue on github for this?

I just created an issue for this.

forrest-rm · 2018-10-01T15:03:33Z

sns_ik_lib/test/sns_ik_acc_test.cpp

+ *************************************************************************************************/
+
+/*
+ * Run the benchmark test on all five versions of the acceleration solver.


There's only one acc solver right now....

Yep. Right..

- removed unnecessary variables in jacobian dot computation - removed hard-coded numbers - enabled sns acc ik by default (only called upon request) - other cleanups

forrest-rm

Overall, it looks fairly good. I just had a few questions.

forrest-rm · 2018-11-06T21:01:39Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+    Eigen::ArrayXd ddotQmax;  // higher joint velocity bound
+    std::vector<double> scaleFactors;
+
+    // variables for base acc ik solver


forrest-rm · 2018-11-06T21:02:29Z

sns_ik_lib/include/sns_ik/sns_acceleration_ik.hpp

+    // The control loop period in seconds
+    void setLoopPeriod(double period) { loop_period = period; }
+
+    // SNS Acceleration IK


Overall, more documentation is still needed.

forrest-rm · 2018-11-06T21:04:08Z

sns_ik_lib/src/sns_acceleration_ik.cpp

+    // check whether the acceleration limits are correctly shaped
+    if (ddotQmin(i) > ddotQmax(i)) {
+      if (ddotQmin(i) > 0) {
+      ROS_ERROR_STREAM("Lower limit has been reversed for J"<< i <<"!\n"<< "jointLimit_Low: " << jointLimit_low(i) 


Need additional indentation

Should this return an error code and/or revert to the previous values?

forrest-rm · 2018-11-06T21:08:45Z

sns_ik_lib/src/sns_ik.cpp


+  // define some constants
+  static const size_t CARTESIAN_DOF = 6;
+  static const size_t NUM_LINKS = 9; // the number of links in robot kinematic chain


Why is this hard-coded and will this break things if the the DOF is different?

Added SNS Acceleration IK Solver Interface with UnitTest

ef410a6

- added sns acceleration ik class and SNS_Base_Acc type - added Jacobian dot computation - created unit test with sawyer model

robodreamer requested review from IanTheEngineer, MatthewPeterKelly, chris-smith and forrest-rm September 27, 2018 20:43

Andy Park added 2 commits September 28, 2018 12:21

Added a missing unit test file that caused Travis to fail

9e6dc91

Correct a log level in the code

c3e21d6

chris-smith reviewed Sep 28, 2018

View reviewed changes

forrest-rm reviewed Oct 1, 2018

View reviewed changes

Addressed the reviewers' comments

66ad233

- removed unnecessary variables in jacobian dot computation - removed hard-coded numbers - enabled sns acc ik by default (only called upon request) - other cleanups

forrest-rm reviewed Nov 6, 2018

View reviewed changes


		SnsAccIkBase::uPtr baseIkSolver;

		SnsIkBase::ExitCode exitCode;

		@@ -189,9 +189,12 @@ SnsIkBase::ExitCode SnsIkBase::computeTaskScalingFactor(const Eigen::MatrixXd& J
		Eigen::ArrayXd jntScaleFactorArr(nJnt_);

Conversation

robodreamer commented Sep 27, 2018 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

chris-smith left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

robodreamer Oct 1, 2018 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

robodreamer Oct 1, 2018 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

forrest-rm left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

forrest-rm left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

robodreamer commented Sep 27, 2018 •

edited

Loading

robodreamer Oct 1, 2018 •

edited

Loading

robodreamer Oct 1, 2018 •

edited

Loading