Cable Robot Updates by gchenfc · Pull Request #331 · borglab/GTDynamics

gchenfc · 2022-01-12T02:38:29Z

This PR merges in updates to the cable robot code. I should have made this PR a long time ago and also split up changes into separate PR's, but I kept procrastinating.

Updates include:

Winch dynamics - includes motor inertia/viscosity/friction which requires:
- an additional factor to model the cable length acceleration
- adding variables for cable acceleration and tension
refactor cdpr_planar_controller to be a Base class that supports both iLQR and "naive" controllers
support both Trapezoidal collocation and Forward-Euler, for dynamics integration
drawing/plotting utilities
scripts related to graffiti / GTGraffiti ICRA 2021 submission
- gerry02 - gerry04
- paint_parse
initialization and Values copying utilities in utils.py
"BlockEliminateSequential" does sequential elimination but able to eliminate multiple variables together, which simplifies the process of extracting the control law from the BayesNet.
- Specifically, we want our control law to be like [torque1; torque2; torque3; torque4] = K * [pose ; twist] so we would like to group the torques (which are currently 4 scalar variables) together and the pose+twist together.
- It is possible to obtain the control law without doing this, but just requires a lot more book-keeping in python and manually populating the K matrix row-by-row.

keyformatter updates

get KeyFormatter printing

…ypo before

* Key functions are no longer internal * values functions remove "Double" template parameter

dellaert · 2022-01-12T02:51:04Z

Well, you have to add (one) reviewer. If you add me, I'll say: can you please split it up in a few, easy to review PRs?

# Conflicts: # gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp # gtdynamics/cablerobot/factors/CableAccelerationFactor.h # gtdynamics/cablerobot/factors/WinchFactor.h # gtdynamics/cablerobot/src/cdpr_planar.py # gtdynamics/cablerobot/src/cdpr_planar_controller.py # gtdynamics/cablerobot/src/test_cdpr_controller_ilqr.py # gtdynamics/cablerobot/src/test_cdpr_planar.py # gtdynamics/cablerobot/src/test_paint_parse.py # gtdynamics/cablerobot/tests/testCableAccelerationFactor.cpp # gtdynamics/cablerobot/utils/CustomWrap.cpp

dellaert · 2026-02-14T03:28:45Z

@gchenfc I merged in #418 so now only 8 files have issues. I'll see whether Codex can fix them.

dellaert · 2026-02-14T03:57:14Z

Update: All c++ tests already worked, and only had to skip a few python tests, that I will not attempt to fix. Asking co-pilot for a quick review, and then we can merge both #418 and then this PR, in order.

Copilot

Pull request overview

This PR brings a large set of updates to the cablerobot module, including new state/control variables (e.g., tension/acceleration), controller refactors, simulation updates, and wrapper/interface additions intended to support winch dynamics and control-law extraction workflows.

Changes:

Extend cablerobot Python utilities to initialize/copy additional variables (joint accel, tension) and add LM parameter helpers.
Refactor planar simulation/controller usage to newer controller/simulator classes and update example scripts accordingly.
Extend the cablerobot wrapper interface (cablerobot.i) with new wrapped utilities (tension keys, elimination helpers) and winch-related factors/params.

Reviewed changes

Copilot reviewed 9 out of 9 changed files in this pull request and generated 11 comments.

Show a summary per file

File	Description
gtdynamics/cablerobot/src/utils.py	Adds initialization/copy helpers for additional variables (accel, tension) and LM params helper.
gtdynamics/cablerobot/src/cdpr_planar_sim.py	Refactors simulation stepping into separate IK/dynamics updates and switches to new priors usage patterns.
gtdynamics/cablerobot/src/test_cdpr_planar_sim.py	Updates imports/class name and disables the test via `unittest.skip`.
gtdynamics/cablerobot/src/test_cdpr_controller_tension_dist.py	Updates imports/class name/test setup and disables the test via `unittest.skip`.
gtdynamics/cablerobot/src/gerry01_planar_tracking.py	Updates to iLQR controller + simulator class; reworks plotting and adds profiling.
gtdynamics/cablerobot/src/gerry00_planar_sim.py	Updates to simulator class API.
gtdynamics/cablerobot/src/cdpr_planar_controller.py	Removes the old planar controller implementation (superseded by new controller modules).
gtdynamics/cablerobot/src/cdpr_planar.py	Minor docstring typo fixes (but is now relied upon by updated call sites).
gtdynamics/cablerobot/cablerobot.i	Adds wrapper declarations for tension keys, elimination helpers, and winch-related types/factors.

Comments suppressed due to low confidence (1)

gtdynamics/cablerobot/src/test_cdpr_controller_tension_dist.py:26

This test is now unconditionally skipped, which removes coverage for the tension-distribution controller path. If it depends on follow-up wrapper work, consider isolating those dependencies behind a feature flag or using a targeted skip condition so the test re-enables automatically once the required APIs are available.

gtdynamics/cablerobot/src/utils.py

Copilot · 2026-02-14T04:01:43Z

gtdynamics/cablerobot/cablerobot.i

+// Tension Key Stuff
+gtdynamics::DynamicsSymbol TensionKey(int j, int t = 0);
+
+void InsertTension(gtsam::Values @values, int j, int t, double value);
+
+void InsertTension(gtsam::Values @values, int j, double value);
+
+double Tension(const gtsam::Values &values, int j, int t = 0);


Parameter names in the .i file should match the C++ declarations exactly for wrapped functions. In CustomWrap.h, TensionKey/InsertTension/Tension use parameter name k for the timestep, but here it is t. Please align these parameter names to avoid wrapper-generation inconsistencies.

gtdynamics/cablerobot/src/test_cdpr_planar_sim.py

Copilot · 2026-02-14T04:01:44Z

gtdynamics/cablerobot/src/cdpr_planar.py

    def priors_ik(self, ks=[], Ts=[], Vs=[]):
        """Creates prior factors which correspond to solving the inverse kinematics problem by
        specifying the Pose/Twist of the end effector.  To be used with kinematics_factors to
        optimize for the joint angles and velocities.

        Args:
            ks (list, optional): Time step indices. Defaults to [].
            Ts (list, optional): List of Poses for each time step. Defaults to [[]].
            Vs (list, optional): List of Twists for each time step. Defaults to [[]].

        Returns:
-            gtsam.NonlinearFactorGraph: The inverve kinematics prior factors
+            gtsam.NonlinearFactorGraph: The inverse kinematics prior factors
        """
        graph = gtsam.NonlinearFactorGraph()


Several modules in cablerobot/src now call Cdpr.priors_ik(..., values=<gtsam.Values>) (e.g. cdpr_planar_sim.py), but this method is still defined with Ts/Vs parameters only and does not accept values. This API mismatch will cause runtime TypeErrors; either add a values parameter/overload (extracting Pose/Twist for each k) or update all call sites to pass Ts/Vs explicitly.

True. I do remember at some point changing the API. In general, I wasn't very consistent about whether to split up the results into multiple Values/list objects (which makes code a bit more readable) or to keep all the results in a single Values object (which is less redundant). There are also significant efficiency considerations, as fetching keys from Values is very slow if done in a python for-loop.

I don't recall which approach I ended up settling on.

Copilot · 2026-02-14T04:01:44Z

gtdynamics/cablerobot/src/cdpr_planar_sim.py

-        # ID initial guess
+        # priors (pose/twist and torque inputs)
+        fg.push_back(cdpr.priors_ik(ks=[k], values=xd))
+        fg.push_back(cdpr.priors_fd(ks=[k], values=u))


update_dynamics uses cdpr.priors_fd(ks=[k], values=u), but Cdpr.priors_fd in cdpr_planar.py currently accepts VAs (twist accelerations) and does not accept a values keyword argument. As written this will throw TypeError; adjust the call to match the existing API or update priors_fd to accept the intended input type.

Suggested change

fg.push_back(cdpr.priors_fd(ks=[k], values=u))

fg.push_back(cdpr.priors_fd(ks=[k], VAs=u))

Same comment as above.

gtdynamics/cablerobot/src/cdpr_planar_sim.py

gtdynamics/cablerobot/cablerobot.i

gtdynamics/cablerobot/src/gerry01_planar_tracking.py

Copilot · 2026-02-14T04:01:45Z

gtdynamics/cablerobot/src/cdpr_planar_sim.py

-            cdpr.priors_ik(ks=[k],
-                           Ts=[gtd.Pose(x, cdpr.ee_id(), k)],
-                           Vs=[gtd.Twist(x, cdpr.ee_id(), k)]))
+        fg.push_back(cdpr.priors_ik(ks=[k], values=xk))


update_kinematics calls cdpr.priors_ik(ks=[k], values=xk), but Cdpr.priors_ik in cdpr_planar.py currently takes Ts/Vs and does not accept a values keyword argument. This will raise a TypeError at runtime; either add a values-based overload/parameter to priors_ik or change this call to pass Ts=[...] and Vs=[...].

Suggested change

fg.push_back(cdpr.priors_ik(ks=[k], values=xk))

fg.push_back(

cdpr.priors_ik(

ks=[k],

Ts=[gtd.Pose(xk, lid, k)],

Vs=[gtd.Twist(xk, lid, k)],

)

)

Can't recall whether everything should be switched to (ks, Ts, Vs) or to values. May need a different agent to assess this in the codebase.

There is another PR where you use @yetongatech 's CmOpt. So maybe take a look at that and see what you decided on? I can try to have the AI bring that up to speed, but I would need your guidance on this particular priors_ik refactor.

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

gchenfc

LGTM

gtdynamics/cablerobot/src/gerry01_planar_tracking.py

gchenfc · 2026-02-18T05:27:06Z

gtdynamics/cablerobot/src/cdpr_planar.py

    def priors_ik(self, ks=[], Ts=[], Vs=[]):
        """Creates prior factors which correspond to solving the inverse kinematics problem by
        specifying the Pose/Twist of the end effector.  To be used with kinematics_factors to
        optimize for the joint angles and velocities.

        Args:
            ks (list, optional): Time step indices. Defaults to [].
            Ts (list, optional): List of Poses for each time step. Defaults to [[]].
            Vs (list, optional): List of Twists for each time step. Defaults to [[]].

        Returns:
-            gtsam.NonlinearFactorGraph: The inverve kinematics prior factors
+            gtsam.NonlinearFactorGraph: The inverse kinematics prior factors
        """
        graph = gtsam.NonlinearFactorGraph()


True. I do remember at some point changing the API. In general, I wasn't very consistent about whether to split up the results into multiple Values/list objects (which makes code a bit more readable) or to keep all the results in a single Values object (which is less redundant). There are also significant efficiency considerations, as fetching keys from Values is very slow if done in a python for-loop.

I don't recall which approach I ended up settling on.

gchenfc · 2026-02-18T05:29:12Z

gtdynamics/cablerobot/src/cdpr_planar_sim.py

-            cdpr.priors_ik(ks=[k],
-                           Ts=[gtd.Pose(x, cdpr.ee_id(), k)],
-                           Vs=[gtd.Twist(x, cdpr.ee_id(), k)]))
+        fg.push_back(cdpr.priors_ik(ks=[k], values=xk))


Can't recall whether everything should be switched to (ks, Ts, Vs) or to values. May need a different agent to assess this in the codebase.

gchenfc · 2026-02-18T05:29:33Z

gtdynamics/cablerobot/src/cdpr_planar_sim.py

-        # ID initial guess
+        # priors (pose/twist and torque inputs)
+        fg.push_back(cdpr.priors_ik(ks=[k], values=xd))
+        fg.push_back(cdpr.priors_fd(ks=[k], values=u))


Same comment as above.

gchenfc · 2026-02-18T05:30:15Z

gtdynamics/cablerobot/src/cdpr_planar_sim.py

+            gtd.InsertTorque(xd, ji, k, gtd.Torque(u, ji, k))
+            gtd.InsertTension(xd, ji, k,
+                                    gtd.Torque(u, ji, k) / cdpr.params.winch_params.radius_)
+            gtd.InsertWrench(xd, cdpr.ee_id(), ji, k, np.zeros(6))


This should be easy for an agent to fix. Might have been a merge conflict I made at some point.

gchenfc · 2026-02-18T05:36:44Z

gtdynamics/cablerobot/src/utils.py

            gtd.InsertJointAngle(zero, j, t, 0)
            gtd.InsertJointVel(zero, j, t, 0)
+            gtd.InsertJointAccel(zero, j, t, 0)
+            gtd.InsertTension(zero, j, t, 0)


fyi for if python unit tests are failing:

This addition makes sense, but might cause some issues if solving a graph which doesn't contain these keys. I can't recall
(a) does gtsam throw an error if the initialization has more keys than the graph?
(b) how did the cablerobot code handle initialization, since different sub-problem-graphs contained different subsets of variables?

Edit: see comment below

To be honest, I don't know where that came from. This could be a merge decision that went the wrong way: maybe you decided to delete it and I didn't know the intent.

gchenfc · 2026-02-18T05:37:42Z

gtdynamics/cablerobot/src/utils.py

+    for key in graph.keyVector():
+        key = gtd.DynamicsSymbol(key)
+        InsertZeroByLabel(z, key, dt)
+    return z


This function makes more sense and is familiar. I don't know what lower-case zerovalues is used for. I don't think it should be used anymore.

dellaert

Yay - now only one PR @gchenfc !!!

gchenfc added 30 commits April 6, 2021 02:31

animated plotting of tracking example

eea92cc

faster simulation

14fe322

a little refactoring

49f21c6

unit test easier priors adding functions

dffa704

values-based priors functions

4252df8

bug fix for some reason

1b32c9d

move cdpr controller ilqr part 1

b50166b

move cdpr controller ilqr part 2

32ce160

unit test for a tension distribution conroller

a4aca11

tension distribution controller - not quite working yet

decb82b

Merge branch 'master' into feature/cablerobot-ilqr

4f09d2a

keyformatter updates

PriorFactorVector6 - use gtd verison

7599c33

paint parsing unit test

b9decde

paint file parsing works

10531d6

fix boolean parsing

3d2c17a

begin trajectory tracking script

b35f1f3

clean up ipython notebook

7d2c038

working simulation with iros logo

5113538

Pott09's mid-tension iLQR

6bf0e85

refactor plotting

ae12b38

expose more options to python notebook

66c58ec

Merge branch 'master' into feature/cablerobot-ilqr

c7fc779

get KeyFormatter printing

unit test for local feedback gains

e8f8929

expand unit test to include twist and feedforward terms

5750d5f

sequential elimination that preserves ordering in bayes net

7a11d03

block elimination

c2ea56f

reduce copy-pasta in EliminateSequential

a1e6d4e

preliminary gains calculation

c27e258

feedforward calculation for gains

9d45d7f

repackage controller gains with ff terms

f40df13

gchenfc added 5 commits September 7, 2021 23:34

comments and easier running from command line

578610f

LQR-style plots for ICRA paper

9595f96

fix - don't need "intermediate control variables" - must have had a t…

a0122ab

…ypo before

Merge branch 'master' into feature/gerry/cdprwinch

d36c9bc

Fix merge issues related to:

e2dc95d

* Key functions are no longer internal * values functions remove "Double" template parameter

gchenfc added 3 commits January 11, 2022 21:53

clean up large data-file diffs

d89c5e0

fix controller docstrings

e9ba1b5

leftover merge fixes

1d34bba

varunagrawal assigned gchenfc Apr 14, 2022

minor syntax updates

3353dc2

gchenfc mentioned this pull request Aug 26, 2022

Manifold optimization on cable robot kinodynamics problem #363

Open

dellaert added a commit that referenced this pull request Feb 13, 2026

Import non-conflicting edits from PR #331

71f452b

dellaert mentioned this pull request Feb 13, 2026

CableRobot additions #418

Merged

dellaert added a commit that referenced this pull request Feb 14, 2026

Import additive files from PR #331

cd7cff3

dellaert changed the base branch from master to codex/pr331-additions-only February 14, 2026 03:26

Skip failing tests

377241b

dellaert requested a review from Copilot February 14, 2026 03:55

Copilot started reviewing on behalf of dellaert February 14, 2026 03:56 View session

Copilot AI reviewed Feb 14, 2026

View reviewed changes

Apply suggestions from code review

fe116a9

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

gchenfc commented Feb 18, 2026

View reviewed changes

Base automatically changed from codex/pr331-additions-only to master February 18, 2026 16:50

Merge branch 'master' into feature/gerry/cdprwinch

3c15854

dellaert approved these changes Feb 18, 2026

View reviewed changes

dellaert merged commit a6a2642 into master Feb 18, 2026
6 checks passed

dellaert deleted the feature/gerry/cdprwinch branch February 18, 2026 18:43

	fg.push_back(cdpr.priors_fd(ks=[k], values=u))
	fg.push_back(cdpr.priors_fd(ks=[k], VAs=u))

Comments

Conversation

gchenfc commented Jan 12, 2022

Uh oh!

dellaert commented Jan 12, 2022

Uh oh!

dellaert commented Feb 14, 2026

Uh oh!

dellaert commented Feb 14, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI Feb 14, 2026

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!

gchenfc left a comment

Choose a reason for hiding this comment

Uh oh!

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!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants