Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Cable Robot Updates #331

Open
wants to merge 77 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
77 commits
Select commit Hold shift + click to select a range
eea92cc
animated plotting of tracking example
gchenfc Apr 6, 2021
14fe322
faster simulation
gchenfc Apr 6, 2021
49f21c6
a little refactoring
gchenfc Apr 6, 2021
dffa704
unit test easier priors adding functions
gchenfc Apr 6, 2021
4252df8
values-based priors functions
gchenfc Apr 6, 2021
1b32c9d
bug fix for some reason
gchenfc Apr 6, 2021
b50166b
move cdpr controller ilqr part 1
gchenfc Apr 6, 2021
32ce160
move cdpr controller ilqr part 2
gchenfc Apr 6, 2021
a4aca11
unit test for a tension distribution conroller
gchenfc Apr 6, 2021
decb82b
tension distribution controller - not quite working yet
gchenfc Apr 6, 2021
4f09d2a
Merge branch 'master' into feature/cablerobot-ilqr
gchenfc Apr 8, 2021
7599c33
PriorFactorVector6 - use gtd verison
gchenfc Apr 8, 2021
b9decde
paint parsing unit test
gchenfc Apr 8, 2021
10531d6
paint file parsing works
gchenfc Apr 11, 2021
3d2c17a
fix boolean parsing
gchenfc Apr 11, 2021
b35f1f3
begin trajectory tracking script
gchenfc Apr 11, 2021
7d2c038
clean up ipython notebook
gchenfc Apr 12, 2021
5113538
working simulation with iros logo
gchenfc Apr 12, 2021
6bf0e85
Pott09's mid-tension iLQR
gchenfc Apr 12, 2021
ae12b38
refactor plotting
gchenfc Apr 12, 2021
66c58ec
expose more options to python notebook
gchenfc Apr 12, 2021
c7fc779
Merge branch 'master' into feature/cablerobot-ilqr
gchenfc Apr 12, 2021
e8f8929
unit test for local feedback gains
gchenfc Apr 13, 2021
5750d5f
expand unit test to include twist and feedforward terms
gchenfc Apr 13, 2021
7a11d03
sequential elimination that preserves ordering in bayes net
gchenfc Apr 14, 2021
c2ea56f
block elimination
gchenfc Apr 14, 2021
a1e6d4e
reduce copy-pasta in EliminateSequential
gchenfc Apr 14, 2021
c27e258
preliminary gains calculation
gchenfc Apr 19, 2021
9d45d7f
feedforward calculation for gains
gchenfc Apr 19, 2021
f40df13
repackage controller gains with ff terms
gchenfc Apr 20, 2021
acf2376
bugfix in unit test: set "median" tension to 0
gchenfc Apr 20, 2021
a7fcd2a
unit test gains using infinite horizon approximation
gchenfc Apr 20, 2021
7499a7d
full IROS trajectory with iLQR
gchenfc Apr 20, 2021
6d0ea28
format comment line in customwrap.cpp
gchenfc Apr 20, 2021
822e27c
minor plotting improvements
gchenfc Apr 21, 2021
918daa7
iros trajectory - higher framerate
gchenfc Apr 21, 2021
8cb5045
id/fd were flip flopped
gchenfc Apr 21, 2021
2a551aa
update cdpr unit test with correct ID/FD and inertia
gchenfc Apr 21, 2021
340978f
python side of ID/FD with joint angles and winch inertia
gchenfc Apr 21, 2021
761168e
winch factor
gchenfc Apr 22, 2021
72d13d0
cable acceleration factor unit test
gchenfc Apr 22, 2021
9f8a0fe
CableAccelFactor passes unit test for simplified
gchenfc Apr 22, 2021
1daaf24
wrap WinchFactor and CableAccelerationFactor
gchenfc Apr 22, 2021
77bfd05
create and wrap TensionKey
gchenfc Apr 22, 2021
7d19163
cdpr with winch inertia passes unit tests :)
gchenfc Apr 22, 2021
fa05de8
update sim and iLQR to pass unit tests
gchenfc Apr 22, 2021
b1a9259
fix torque real values and plotting
gchenfc Apr 22, 2021
d618e65
add functions to visualize gains
gchenfc Apr 22, 2021
45599ad
update "zeroValues" to include joint accel and tension
gchenfc Apr 26, 2021
c1173fa
write out gains to .h file
gchenfc Apr 26, 2021
149501a
switch to ATL painting and make EE mass more realistic
gchenfc May 5, 2021
103f564
add raw trajectories to source control (git lfs)
gchenfc May 5, 2021
9b961b1
Merge branch 'master' into feature/gerry/cdprwinch
gchenfc Aug 26, 2021
6c8233f
WIP: fix some unit tests:
gchenfc Aug 28, 2021
e0508f0
remove debug statements
gchenfc Aug 28, 2021
8879146
WIP: bugfix in gtsam python wrapper: see branch:
gchenfc Aug 28, 2021
4c30d2c
WIP: cdpr_planar (post gtsam bugfix)
gchenfc Aug 28, 2021
23d2eda
WIP: cdpr_planar_sim (post gtsam bugfix)
gchenfc Aug 28, 2021
8f46cef
WIP: cdpr_controller_tension_dist (post gtsam bugfix)
gchenfc Aug 28, 2021
08997a5
WIP: cdpr_controller_ilqr (post gtsam bugfix)
gchenfc Aug 28, 2021
a44e6a1
cleanup
gchenfc Aug 29, 2021
dc709fc
parameter tweaking
gchenfc Aug 29, 2021
2b0e523
minor refactoring
gchenfc Sep 7, 2021
d1bb841
initial code: per-stroke optimization
gchenfc Sep 7, 2021
d33f09e
optimize utils.InitValues
gchenfc Sep 8, 2021
cee41cc
iLQR "initial guess" - doesn't help that much
gchenfc Sep 8, 2021
a3ba8be
make iLQR way faster by using less strict optim params
gchenfc Sep 8, 2021
6a8665c
update stroke tracking
gchenfc Sep 8, 2021
578610f
comments and easier running from command line
gchenfc Sep 8, 2021
9595f96
LQR-style plots for ICRA paper
gchenfc Oct 3, 2021
a0122ab
fix - don't need "intermediate control variables" - must have had a t…
gchenfc Oct 21, 2021
d36c9bc
Merge branch 'master' into feature/gerry/cdprwinch
gchenfc Jan 12, 2022
e2dc95d
Fix merge issues related to:
gchenfc Jan 12, 2022
d89c5e0
clean up large data-file diffs
gchenfc Jan 12, 2022
e9ba1b5
fix controller docstrings
gchenfc Jan 12, 2022
1d34bba
leftover merge fixes
gchenfc Jan 12, 2022
3353dc2
minor syntax updates
gchenfc Jun 3, 2022
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
move cdpr controller ilqr part 1
  • Loading branch information
gchenfc committed Apr 6, 2021
commit b50166b96b1f2cb2db8d13fe7539b64bd0d5d5c5
Original file line number Diff line number Diff line change
@@ -15,28 +15,9 @@
import gtdynamics as gtd
import numpy as np
import utils
from cdpr_planar_controller import CdprControllerBase

class CdprControllerBase:
"""Interface for cable robot controllers
"""
@property
def update(self, values, t):
"""gives the new control input given current measurements

Args:
values (gtsam.Values): values object will contain at least the current Pose and Twist,
but should often also include the current joint angles and velocities
t (int): The current time index (discrete time index)

Returns:
gtsam.Values: A values object which contains the joint torques for this time step.

Raises:
NotImplementedError: Derived classes must override this function
"""
raise NotImplementedError("CdprControllers need to implement the `update` function")

class CdprController(CdprControllerBase):
class CdprControllerIlqr(CdprControllerBase):
"""Precomputes the open-loop trajectory
then just calls on that for each update.
"""
@@ -56,13 +37,14 @@ def __init__(self, cdpr, x0, pdes=[], dt=0.01, Q=None, R=np.array([1.])):
self.pdes = pdes
self.dt = dt

# initial guess
x0 = utils.zerovalues(cdpr.ee_id(), ts=range(len(pdes)), dt=dt)
# create iLQR graph
fg = self.create_ilqr_fg(cdpr, x0, pdes, dt, Q, R)
# initial guess
init = utils.zerovalues(cdpr.ee_id(), range(len(pdes)), dt=dt)
# optimize
self.optimizer = gtsam.LevenbergMarquardtOptimizer(fg, init)
self.result = self.optimizer.optimize()
self.optimizer = gtsam.LevenbergMarquardtOptimizer(fg, x_guess)
# self.result = self.optimizer.optimize()
self.result = x_guess
self.fg = fg

def update(self, values, t):
4 changes: 2 additions & 2 deletions gtdynamics/cablerobot/src/test_cdpr_planar_controller.py
Original file line number Diff line number Diff line change
@@ -17,7 +17,7 @@
from gtsam import Pose3, Rot3
import numpy as np
from cdpr_planar import Cdpr
from cdpr_planar_controller import CdprController
from cdpr_controller_ilqr import CdprControllerIlqr
from cdpr_planar_sim import CdprSimulator
from gtsam.utils.test_case import GtsamTestCase

@@ -33,7 +33,7 @@ def testTrajFollow(self):

x_des = [Pose3(Rot3(), (1.5+k/20.0, 0, 1.5)) for k in range(9)]
x_des = x_des[0:1] + x_des
controller = CdprController(cdpr, x0=x0, pdes=x_des, dt=0.1)
controller = CdprControllerIlqr(cdpr, x0=x0, pdes=x_des, dt=0.1)

sim = CdprSimulator(cdpr, x0, controller, dt=0.1)
result = sim.run(N=10)