Move more tests/ Fix targets

README.md

@@ -97,11 +97,14 @@ $ make help
 
 # Run all C++ tests or grouped test sets
 $ make check
-$ make check.tests
 $ make check.kinematics
 $ make check.mechanics
 $ make check.statics
 $ make check.dynamics
+$ make check.cablerobot
+$ make check.jumpingrobot
+$ make check.pandarobot
+$ make check.tests # other tests
 
 # Run a single C++ test executable target
 $ make testTwistAccelFactor.run

examples/CMakeLists.txt

@@ -21,9 +21,6 @@ foreach(EXAMPLE ${EXAMPLE_SUBDIRS})
   add_subdirectory(${EXAMPLE})
 endforeach()
 
-# Convenience alias: build all examples via a check-style target name.
-add_custom_target(check.examples DEPENDS examples)
-
 # --- Process Python Animation Scripts ---
 # This automatically finds 'animate.py' scripts and creates .animate targets.
 message(STATUS "Searching for python animation scripts...")

gtdynamics/cablerobot/tests/CMakeLists.txt

@@ -1 +1 @@
-gtsamAddTestsGlob(tests "test*.cpp" "" "gtdynamics")
+gtsamAddTestsGlob(cablerobot "test*.cpp" "" "gtdynamics")

gtdynamics/jumpingrobot/tests/CMakeLists.txt

@@ -1 +1 @@
-gtsamAddTestsGlob(tests "test*.cpp" "" "gtdynamics")
+gtsamAddTestsGlob(jumpingrobot "test*.cpp" "" "gtdynamics")

gtdynamics/kinematics/KinematicsInterval.cpp

@@ -149,9 +149,12 @@ Values Kinematics::interpolate<Interval>(
     const ContactGoals& contact_goals1,
     const ContactGoals& contact_goals2) const {
   Values result;
-  const double dt = 1.0 / (interval.k_start - interval.k_end);  // 5 6 7 8 9 [10
+  const double denominator =
+      static_cast<double>(interval.k_end - interval.k_start);
   for (size_t k = interval.k_start; k <= interval.k_end; k++) {
-    const double t = dt * (k - interval.k_start);
+    const double t = denominator > 0.0
+                         ? static_cast<double>(k - interval.k_start) / denominator
+                         : 0.0;
     ContactGoals goals;
     transform(contact_goals1.begin(), contact_goals1.end(),
               contact_goals2.begin(), std::back_inserter(goals),

gtdynamics/kinematics/KinematicsSlice.cpp

@@ -14,10 +14,10 @@
 #include <gtdynamics/kinematics/ContactHeightFactor.h>
 #include <gtdynamics/kinematics/ContactKinematicsTwistFactor.h>
 #include <gtdynamics/kinematics/JointLimitFactor.h>
+#include <gtdynamics/kinematics/Kinematics.h>
 #include <gtdynamics/kinematics/PointGoalFactor.h>
 #include <gtdynamics/kinematics/PoseFactor.h>
 #include <gtdynamics/kinematics/TwistFactor.h>
-#include <gtdynamics/kinematics/Kinematics.h>
 #include <gtdynamics/utils/Slice.h>
 #include <gtsam/linear/Sampler.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -371,6 +371,7 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
   // graph.addPrior<gtsam::Pose3>(PoseKey(0, slice.k),
   // gtsam::Pose3(), nullptr);
 
+  // TODO(frank): we should allow warm start when used in interval context.
   auto initial_values = initialValues(slice, robot);
 
   return optimize(graph, constraints, initial_values);

tests/testKinematicsInterval.cpp → gtdynamics/kinematics/tests/testKinematicsInterval.cpp

@@ -26,6 +26,44 @@ using std::string;
 
 #include "contactGoalsExample.h"
 
+namespace {
+
+ContactGoals InterpolateContactGoals(const ContactGoals& contact_goals_start,
+                                     const ContactGoals& contact_goals_end,
+                                     double alpha) {
+  ContactGoals contact_goals;
+  std::transform(
+      contact_goals_start.begin(), contact_goals_start.end(),
+      contact_goals_end.begin(), std::back_inserter(contact_goals),
+      [alpha](const ContactGoal& start_goal, const ContactGoal& end_goal) {
+        return ContactGoal{
+            start_goal.point_on_link,
+            (1.0 - alpha) * start_goal.goal_point + alpha * end_goal.goal_point};
+      });
+  return contact_goals;
+}
+
+gtsam::Values ManualInterpolateByInverse(
+    const Kinematics& kinematics, const Interval& interval, const Robot& robot,
+    const ContactGoals& contact_goals_start,
+    const ContactGoals& contact_goals_end) {
+  gtsam::Values result;
+  const double denominator =
+      static_cast<double>(interval.k_end - interval.k_start);
+  for (size_t k = interval.k_start; k <= interval.k_end; ++k) {
+    const double alpha =
+        denominator > 0.0
+            ? static_cast<double>(k - interval.k_start) / denominator
+            : 0.0;
+    const auto goals =
+        InterpolateContactGoals(contact_goals_start, contact_goals_end, alpha);
+    result.insert(kinematics.inverse(Slice(k), robot, goals));
+  }
+  return result;
+}
+
+}  // namespace
+
 TEST(Interval, InverseKinematics) {
   // Load robot and establish contact/goal pairs
   // TODO(frank): the goals for contact will differ for a Interval vs Slice.
@@ -73,7 +111,7 @@ TEST(Interval, Interpolate) {
   parameters.method = OptimizationParameters::Method::SOFT_CONSTRAINTS;
   Kinematics kinematics(parameters);
   auto result1 = kinematics.inverse(Slice(5), robot, contact_goals);
-  auto result2 = kinematics.inverse(Slice(9), robot, contact_goals);
+  auto result2 = kinematics.inverse(Slice(9), robot, contact_goals2);
 
   // Create a kinematic trajectory over timesteps 5, 6, 7, 8, 9 that
   // interpolates between goal configurations at timesteps 5 and 9.
@@ -85,6 +123,37 @@ TEST(Interval, Interpolate) {
   EXPECT(assert_equal(Pose(result2, 0, 9), Pose(result, 0, 9)));
 }
 
+TEST(Interval, InterpolateMatchesManualInverseLoop) {
+  using namespace contact_goals_example;
+
+  // Move RF foot by 10 cm in +x at the end of interval.
+  auto contact_goals2 = contact_goals;
+  contact_goals2[2] = {{RF, contact_in_com}, {0.4, -0.16, -0.2}};
+
+  KinematicsParameters parameters;
+  parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN;
+  Kinematics kinematics(parameters);
+
+  const Interval interval(5, 9);
+  const auto manual_result = ManualInterpolateByInverse(
+      kinematics, interval, robot, contact_goals, contact_goals2);
+  const auto interpolate_result =
+      kinematics.interpolate(interval, robot, contact_goals, contact_goals2);
+
+  // Interpolate should match an explicit per-k call to inverse on interpolated
+  // goal points.
+  constexpr double tol = 1e-5;
+  for (size_t k = interval.k_start; k <= interval.k_end; ++k) {
+    for (const auto& joint : robot.joints()) {
+      const auto key = JointAngleKey(joint->id(), k);
+      EXPECT(manual_result.exists(key));
+      EXPECT(interpolate_result.exists(key));
+      EXPECT_DOUBLES_EQUAL(manual_result.at<double>(key),
+                           interpolate_result.at<double>(key), tol);
+    }
+  }
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);

gtdynamics/pandarobot/tests/CMakeLists.txt

@@ -1 +1 @@
-gtsamAddTestsGlob(tests "test*.cpp" "" "gtdynamics")
+gtsamAddTestsGlob(pandarobot "test*.cpp" "" "gtdynamics")

python/notebooks/inverse_kinematics.ipynb

@@ -1,53 +1,31 @@
 {
- "metadata": {
-  "language_info": {
-   "codemirror_mode": {
-    "name": "ipython",
-    "version": 3
-   },
-   "file_extension": ".py",
-   "mimetype": "text/x-python",
-   "name": "python",
-   "nbconvert_exporter": "python",
-   "pygments_lexer": "ipython3",
-   "version": "3.9.6"
-  },
-  "orig_nbformat": 2,
-  "kernelspec": {
-   "name": "python3",
-   "display_name": "Python 3.9.6 64-bit ('base': conda)"
-  },
-  "interpreter": {
-   "hash": "95ec9ec1504d83f612128e0fb229072f90bbb4cb09d9d5d93b5dd26e0ca2cfd1"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 2,
  "cells": [
   {
    "cell_type": "code",
    "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": [
     "import gtdynamics as gtd\n",
     "from gtdynamics import ContactGoal, PointOnLink, Slice, Interval\n",
     "from gtsam import Pose3, Point3"
-   ],
-   "outputs": [],
-   "metadata": {}
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": [
     "# Load the vision 60 quadruped by Ghost robotics: https://youtu.be/wrBNJKZKg10\n",
     "robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + \"/vision60.urdf\");"
-   ],
-   "outputs": [],
-   "metadata": {}
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": [
     "# feet\n",
     "contact_in_com =(0.14, 0, 0)\n",
@@ -64,55 +42,117 @@
     "    ContactGoal(RH, [-0.4, -0.16, 0])\n",
     "    ]\n",
     "print(contact_goals)"
-   ],
-   "outputs": [],
-   "metadata": {}
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
-   "source": [
-    "kinematics = gtd.Kinematics()\n",
-    "result = kinematics.inverse(Slice(4), robot, contact_goals)\n",
-    "print(result)"
-   ],
+   "metadata": {},
    "outputs": [],
-   "metadata": {}
+   "source": [
+    "parameters = gtd.KinematicsParameters()\n",
+    "# parameters.method = gtd.KinematicsParameters.Method.SOFT_CONSTRAINTS # not yet wrapped\n",
+    "kinematics = gtd.Kinematics(parameters)\n",
+    "result = kinematics.inverse(Slice(4), robot, contact_goals)"
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": [
     "for goal in contact_goals:\n",
     "    print(goal.link().name(), goal.satisfied(result,k=4,tol=1e-3))"
-   ],
-   "outputs": [],
-   "metadata": {}
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": [
     "# interpolate\n",
+    "\n",
     "contact_goals2 = [\n",
     "    ContactGoal(LH, [-0.4, 0.16, 0]),\n",
     "    ContactGoal(LF, [0.3, 0.16, 0]),\n",
-    "    ContactGoal(RF, [0.4, -0.16, 0]), # 10 cm on!\n",
-    "    ContactGoal(RH, [-0.4, -0.16, 0])\n",
-    "    ]\n",
-    "interpolated_values = kinematics.interpolate(Interval(4,8), robot, contact_goals, contact_goals2)"
-   ],
-   "outputs": [],
-   "metadata": {}
+    "    ContactGoal(RF, [0.3 + 0.10, -0.16, 0]),  # 10 cm on!\n",
+    "    ContactGoal(RH, [-0.4, -0.16, 0]),\n",
+    "]\n",
+    "\n",
+    "k_start, k_end = 4, 8\n",
+    "interpolated_values = kinematics.interpolate(\n",
+    "    Interval(k_start, k_end), robot, contact_goals, contact_goals2\n",
+    ")\n",
+    "# interpolated_values.print(\"interpolated_values\", gtd.GTDKeyFormatter)"
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": [
-    "print(interpolated_values)"
-   ],
+    "import plotly.graph_objects as go\n",
+    "\n",
+    "fig = go.Figure()\n",
+    "\n",
+    "for joint in robot.joints():\n",
+    "    joint_id = joint.id()\n",
+    "    ks = []\n",
+    "    angles = []\n",
+    "    for k in range(k_start, k_end + 1):\n",
+    "        key = gtd.JointAngleKey(joint_id, k)\n",
+    "        if interpolated_values.exists(key):\n",
+    "            ks.append(k)\n",
+    "            angles.append(interpolated_values.atDouble(key))\n",
+    "\n",
+    "    if angles:\n",
+    "        fig.add_trace(go.Scatter(x=ks, y=angles, mode=\"lines+markers\", name=joint.name()))\n",
+    "\n",
+    "fig.update_layout(\n",
+    "    title=\"Interpolated Joint Angles\",\n",
+    "    xaxis_title=\"k\",\n",
+    "    yaxis_title=\"angle (rad)\",\n",
+    "    template=\"plotly_white\",\n",
+    ")\n",
+    "fig.show()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
    "outputs": [],
-   "metadata": {}
+   "source": [
+    "for k in range(k_start, k_end + 1):\n",
+    "    print(f\"--- k={k} ---\")\n",
+    "    for goal in contact_goals:\n",
+    "        print(goal.link().name(), goal.satisfied(interpolated_values,k=k,tol=1e-3))"
+   ]
   }
- ]
-}
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "py312",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.12.6"
+  },
+  "orig_nbformat": 2
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}