Added tests for chains #351

danbarla · 2022-04-29T15:50:37Z

This PR adds tests to testChain.cpp.
The first test I added compares between classic analysis (wrench constraint for every link) of an A1 leg and kinematic chain analysis. This comes after I had trouble when comparing bigger factor graphs (classic way and chain way) and I had to go simple and follow the math. There is also an overleaf with detailed explanations.

The second test I added sets a basic graph for one leg of the A1 quadruped using DynamicsGraph, and another graph using Chain constraints. the test compares the results and checks them to be identical.

The third test I added compares a Dynamics Graph and a Chain graph for all 4 legs of the A1, for 1 time slice and 4 legs on the ground (robot standing still)

dellaert

I tried to read the overleaf but I am confused. I propose you

use L, U, H, T for the link frames, not ABC. Hard enough to read without translating.
when using a word in the text, always but symbol, and vice versa
most importantly: I think the parent-child relationship should be consistent, from the trunk (great-grandparent) to hip (grandparent) to upper (parent) to lower (child). So, the wrench unknowns will be F^1_L, F^2_U, F^3_H.
finally, do the joint numbers agree with A1 URDF? This is maybe the meta-advice: avoid introducing extra translation tables between symbols will make debugging and understanding more clear.

I'll take another look after you make those changes in doc and corresponding symbols in code. Hopefully just global replaces...

tests/testChain.cpp

danbarla · 2022-05-06T12:42:20Z

adapted overleaf and symbols in code.

dellaert · 2022-05-06T18:47:33Z

@danbarla can you give me write access on the overleaf? Mostly for commenting but when I see a typo I can immediately fix...

dellaert

Still puzzled, in both overleaf and code, where the wrench from the ground is. Ultimately, we need a force exerted by the ground to keep the robot from falling, right? In the youtube video I did that by creating a "virtual joint" at the contact point and then asserting (via a constraint) that there was no moment exteted by the joint on the child link. I think in that case the child link is the "floor".

tests/testChain.cpp

danbarla · 2022-05-11T20:18:20Z

overleaf and code updated with the ground constraint.

dellaert

Unfortunately I don't have cycles now to do a thorough review. If you think everything is correct, feel free to merge.

varunagrawal · 2022-07-07T19:10:51Z

@danbarla should we merge this?

danbarla · 2022-07-07T19:16:21Z

@danbarla should we merge this?

I am actively working on this branch. I will merge it next week.

…ethods calculate the end effector wrench for a massless leg using chain. Adapted ChainDynamicsGraph class and ChainInitializer.

…ld graph and new graph

danbarla · 2023-06-01T18:10:38Z

Hi @dellaert ,
I am ready to merge this PR.
It was opened and approved a year ago, but a lot has changed since then.
Most of the new code and constraints which I used for the paper and thesis was written and pushed to this PR after it was approved the first time. I realize it wasn't the best thing to do :) .
The hardcore changes are in the Chain and ChainDynamicsGraph classes.

varunagrawal · 2023-10-11T22:08:24Z

gtdynamics/universal_robot/Joint.h

+  /**
+   * @brief Create expression for child wrench adjoint from parent link
+   */
+  gtsam::Vector6_ childAdjointWrench(gtsam::Vector6_ &wrench_p,


Should this be named childWrenchAdjoint so it matches the docstring?

varunagrawal · 2023-10-11T22:09:07Z

gtdynamics/utils/ChainInitializer.cpp

-    InsertTwist(&values, i, t, sampler.sample());
-    InsertTwistAccel(&values, i, t, sampler.sample());
+    const int i = link->id();
+    if (i==0 || i==3 || i==6 || i==9 || i==12)


Not sure what's happening here. The link IDs are not guaranteed to be consistent across robots. If these IDs are for the feet, they should be passed in as an argument.

varunagrawal · 2023-10-11T22:09:37Z

gtdynamics/utils/ChainInitializer.cpp

                                        JointVelKey(j, t), JointAccelKey(j, t)};
    for (size_t i = 0; i < keys.size(); i++)
      values.insert(keys[i], sampler.sample()[0]);
  }

-  if (contact_points) {
-    for (auto&& cp : *contact_points) {
+  //if (contact_points) {


Any reason this is commented out?

varunagrawal · 2023-10-11T22:10:04Z

gtdynamics/utils/ChainInitializer.cpp

+    const int i = link->id();
+    if (i==0 || i==3 || i==6 || i==9 || i==12)
+      InsertPose(&values, i, t, AddGaussianNoiseToPose(link->bMcom(), sampler));
+    //InsertTwist(&values, i, t, sampler.sample());


Since you've moved these lines outside the if statement, kill these commented out lines?

varunagrawal · 2023-10-11T22:11:23Z

tests/testChain.cpp

@@ -303,6 +308,310 @@ TEST(Chain, DynamicalEquality3_H_angles_chain1) {
  }
 }

+// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - first type


Nitpick: doxygen comments should be

/// Single line docstring

or

/** * Multiline docstring */

varunagrawal · 2023-10-11T22:23:12Z

examples/example_a1_walking/sim.py

-#plt.show()
+plt.show()
+#plt.rc('pgf', texsystem='pdflatex')
+#plt.savefig('/home/dan/Desktop/my_papers/thesis/create_simulation/pyb_sim.pgf')


Please kill commented code if you're not using it.

varunagrawal · 2023-10-11T22:23:45Z

examples/example_a1_walking/sim.py

@@ -12,11 +12,12 @@

 _ = p.connect(p.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())  # To load plane SDF.
+print(pybullet_data.getDataPath())


This whole file needs some cleaning up IMHO.

varunagrawal · 2023-10-11T22:24:08Z

examples/example_a1_walking/main_combined.cpp

+
+using namespace gtdynamics;
+
+// Returns a Trajectory object for a single a1 walk cycle.


Please run the formatter on this?

varunagrawal · 2023-10-11T22:24:26Z

examples/example_a1_walking/main.cpp

+        objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles));
+      }
+
+    // Add prior on A1 lower joint angles


Please kill commented out code and run the formatter?

varunagrawal · 2023-10-11T22:25:09Z

examples/example_a1_walking/CMakeLists.txt

+  ${WALK_FORWARD_COMBINED}.run
+  COMMAND ./${WALK_FORWARD_COMBINED}
+  DEPENDS ${WALK_FORWARD_COMBINED}
+  WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME})


Newline please

danbarla added 2 commits April 29, 2022 18:08

added tests for chain constraints

c4c241c

Merge branch 'master' into feature/chain_with_robot_test

9b78a8a

danbarla requested a review from dellaert April 29, 2022 16:50

dellaert reviewed Apr 30, 2022

View reviewed changes

tests/testChain.cpp Outdated Show resolved Hide resolved

tests/testChain.cpp Outdated Show resolved Hide resolved

tests/testChain.cpp Outdated Show resolved Hide resolved

tests/testChain.cpp Outdated Show resolved Hide resolved

changed chain order, adapted chain constraint

1428654

danbarla requested a review from dellaert May 6, 2022 14:47

dellaert reviewed May 6, 2022

View reviewed changes

tests/testChain.cpp Outdated Show resolved Hide resolved

danbarla added 2 commits May 11, 2022 22:14

added ground contact point constraint

36b461b

added regression comments

6bef429

danbarla requested a review from dellaert May 11, 2022 20:18

dellaert approved these changes May 11, 2022

View reviewed changes

one leg comparison test added

7aca1e0

varunagrawal assigned danbarla Jul 7, 2022

danbarla and others added 12 commits July 10, 2022 13:16

added 4 legs standing robot comparison test

3ac10ec

Merge branch 'master' into feature/chain_with_robot_test

1848c1c

chain graph class added

6766b12

Added PoeConstraint3 and PoeEquality3 methods to Chain class. These m…

aa58640

…ethods calculate the end effector wrench for a massless leg using chain. Adapted ChainDynamicsGraph class and ChainInitializer.

fixed testChain for PoeEquality3 and example_a1_walking now runs an o…

11e3898

…ld graph and new graph

Merge branch 'master' into feature/chain_with_robot_test

4d3459d

small fix

cd7620f

added q-factors to ChainDynamicsGraph

baa74e2

small changes to simulation

ae669af

Merge branch 'master' into feature/chain_with_robot_test

5ad7ac0

boost removals

f417fcc

Merge branch 'master' into feature/chain_with_robot_test

068beff

danbarla requested a review from dellaert June 1, 2023 18:10

varunagrawal reviewed Oct 11, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Added tests for chains #351

Added tests for chains #351

danbarla commented Apr 29, 2022 •

edited

Loading

dellaert left a comment •

edited

Loading

danbarla commented May 6, 2022

dellaert commented May 6, 2022

dellaert left a comment

danbarla commented May 11, 2022

dellaert left a comment

varunagrawal commented Jul 7, 2022

danbarla commented Jul 7, 2022

danbarla commented Jun 1, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023

varunagrawal Oct 11, 2023


		using namespace gtdynamics;

		// Returns a Trajectory object for a single a1 walk cycle.

Added tests for chains #351

Are you sure you want to change the base?

Added tests for chains #351

Conversation

danbarla commented Apr 29, 2022 • edited Loading

dellaert left a comment • edited Loading

Choose a reason for hiding this comment

danbarla commented May 6, 2022

dellaert commented May 6, 2022

dellaert left a comment

Choose a reason for hiding this comment

danbarla commented May 11, 2022

dellaert left a comment

Choose a reason for hiding this comment

varunagrawal commented Jul 7, 2022

danbarla commented Jul 7, 2022

danbarla commented Jun 1, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

danbarla commented Apr 29, 2022 •

edited

Loading

dellaert left a comment •

edited

Loading