diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index cbec686b753e00fb945018a65c3a29abc042fc33..e8f0cf9924bbd4c123fcdc9cf58e2b3c93cdaf56 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index e00e5fe4b51234988fa9300a62849b129253b847..82aa642f4eee74e77dad389ccccdaa2651892031 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 444ea98d9c02d295ecbeafa3279dd2f119ea304b..47977c2bfe829a356d1256c1170df6910239d235 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -63,7 +63,7 @@ def moveJP(args, robot, q_desired):
print("MoveJP done: convergence achieved, reached destionation!")
-def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data):
+def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
breakFlag = False
save_past_dict = {}
log_item = {}
@@ -73,10 +73,21 @@ def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data
# which is pretty much true
t = i * robot.dt
# take the future (next) one
+ # TODO: turn this into interpolation
t_index = int(np.ceil(t / reference['dt']))
+ # TODO: move to a different function later
+ if t_index >= len(reference['qs']):
+ t_index = len(reference['qs']) - 1
+ if stop_at_final:
+ reference['vs'][t_index] = np.zeros(len(reference['vs'][t_index]))
# TODO: check if that's really the last
- if t_index == len(reference['qs']) - 1:
+ #if t_index == len(reference['qs']) - 1:
+ # breakFlag = True
+
+ # TODO NOTE TODO TODO: remove/change
+ if (t_index == len(reference['qs']) - 1) and \
+ (np.linalg.norm(q - reference['qs'][-1]) < 1e-2):
breakFlag = True
error_q = reference['qs'][t_index] - q
@@ -91,6 +102,8 @@ def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data
log_item['error_q'] = error_q
log_item['error_v'] = error_v
+ log_item['q'] = q
+ log_item['v'] = v
log_item['reference_q'] = reference['qs'][t_index]
log_item['reference_v'] = reference['vs'][t_index]
@@ -98,10 +111,12 @@ def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data
def jointTrajFollowingPD(args, robot, reference):
# we're not using any past data or logging, hence the empty arguments
- controlLoop = partial(jointTrajFollowingPDControlLoop, robot, reference)
+ controlLoop = partial(jointTrajFollowingPDControlLoop, args.stop_at_final, robot, reference)
log_item = {
'error_q' : np.zeros(robot.model.nq),
'error_v' : np.zeros(robot.model.nv),
+ 'q' : np.zeros(robot.model.nq),
+ 'v' : np.zeros(robot.model.nv),
'reference_q' : np.zeros(robot.model.nq),
'reference_v' : np.zeros(robot.model.nv)
}
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 340d36e4ab4175af420bf3c439c96d7e98964ca1..3e4cbf9b0c26d7698bf4e363376a2526967d6205 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -452,7 +452,8 @@ class RobotManager:
self.dt = 1 / self.update_rate
# you better not give me crazy stuff
# and i'm not clipping it, you're fixing it
- assert args.acceleration <= 1.7 and args.acceleration > 0.0
+ self.MAX_ACCELERATION = 1.7
+ assert args.acceleration <= self.MAX_ACCELERATION and args.acceleration > 0.0
# this is the number passed to speedj
self.acceleration = args.acceleration
# NOTE: this is evil and everything only works if it's set to 1
@@ -816,13 +817,13 @@ class RobotManager:
else:
# this one takes all 8 elements of qd since we're still in pinocchio
# this is ugly, todo: fix
- if len(qd) == 6:
- qd = qd_cmd.reshape((6,))
- qd = list(qd)
- qd.append(0.0)
- qd.append(0.0)
- qd = np.array(qd)
- self.v_q = qd
+ qd = qd[:6]
+ qd = qd_cmd.reshape((6,))
+ qd = list(qd)
+ qd.append(0.0)
+ qd.append(0.0)
+ qd = np.array(qd)
+ self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
def openGripper(self):
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/optimal_control.py
index c8daeb8d424934a5f3602a2d0557460245301e49..e3fb969279a98988ac8ad84fff12e0db0b278096 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/optimal_control.py
@@ -4,6 +4,9 @@ import crocoddyl
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
import argparse
from ur_simple_control.basics.basics import jointTrajFollowingPD
+from ur_simple_control.visualize.visualize import plotFromDict
+import example_robot_data
+import time
def get_OCP_args(parser : argparse.ArgumentParser):
@@ -11,18 +14,31 @@ def get_OCP_args(parser : argparse.ArgumentParser):
help="time length between state-control-pair decision variables")
parser.add_argument("--n-knots", type=int, default=100, \
help="number of state-control-pair decision variables")
+ parser.add_argument("--stop-at-final", type=int, default=True, \
+ help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
+ if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
return parser
# TODO: use fddp and incorporate torque (i.e. velocity constraints)
# --> those will correspond to max_qd and acceleration attributes in robotmanager
def IKOCP(args, robot : RobotManager, goal : pin.SE3):
+ # create torque bounds which correspond to percentage
+ # of maximum allowed acceleration
+ # NOTE: idk if this makes any sense, but let's go for it
+ #print(robot.model.effortLimit)
+ #exit()
+ #robot.model.effortLimit = robot.model.effortLimit * (args.acceleration / robot.MAX_ACCELERATION)
+ #robot.model.effortLimit = robot.model.effortLimit * 0.01
+ #robot.data = robot.model.createData()
# starting state
x0 = np.concatenate([robot.getQ(), robot.getQd()])
state = crocoddyl.StateMultibody(robot.model)
# command input IS torque
actuation = crocoddyl.ActuationModelFull(state)
- # we will be summing 3 different costs
+
+ # we will be summing 4 different costs
+ # first 3 are for tracking, state and control regulation
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
uResidual = crocoddyl.ResidualModelControl(state, state.nv)
@@ -43,24 +59,56 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# cost 3) x residual (overall amount of movement)
xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
# put these costs into the running costs
- runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
- runningCostModel.addCost("xReg", xRegCost, 1e-1)
- runningCostModel.addCost("uReg", uRegCost, 1e-1)
+ runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ runningCostModel.addCost("xReg", xRegCost, 1e-3)
+ runningCostModel.addCost("uReg", uRegCost, 1e-3)
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
- terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e3)
+ terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ terminalCostModel.addCost("uReg", uRegCost, 1e3)
+
+ # the 4th cost is for defining bounds via costs
+ # NOTE: could have gotten the same info from state.lb and state.ub.
+ # the first state is unlimited there idk what that means really,
+ # but the arm's base isn't doing a full rotation anyway, let alone 2 or more
+ xlb = np.concatenate([
+ robot.model.lowerPositionLimit,
+ -1 * robot.model.velocityLimit])
+ xub = np.concatenate([
+ robot.model.upperPositionLimit,
+ robot.model.velocityLimit])
+ bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
+ xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
+ xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
+ limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
+ runningCostModel.addCost("limitCost", limitCost, 1e3)
+ terminalCostModel.addCost("limitCost", limitCost, 1e3)
+ # NOTE: i have no idea how to incorporate this into anything
+ # we need to add state constraints for things to make sense
+ # NOTE!!!: there are no hard inequality constraints on states
+ # in what's implemented in crocoddyl.
+ # INSTEAD, you define quadratic barier-type costs for that.
+ # there are box constraints on control input though,
+ # but i'm pretty sure i'm not using those correctly.
+ #x_constraint_lower = x0.copy()
+ #x_constraint_upper = x0.copy()
+ #x_constraint_lower[:robot.model.nq] = -2 * np.pi
+ #x_constraint_upper[:robot.model.nq] = 2 * np.pi
+ #x_constraint_lower[robot.model.nq:] = -1 * robot.max_qd
+ #x_constraint_upper[robot.model.nq:] = robot.max_qd
+ #state_constraint = crocoddyl.ConstraintModelResidual(state, xResidual, x_constraint_lower, x_constraint_upper)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFreeFwdDynamics.
runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeFwdDynamics(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
state, actuation, runningCostModel
),
args.ocp_dt,
)
terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeFwdDynamics(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
state, actuation, terminalCostModel
),
0.0,
@@ -69,25 +117,49 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, [runningModel] * args.n_knots, terminalModel)
# and the solver
- solver = crocoddyl.SolverFDDP(problem)
+ solver = crocoddyl.SolverBoxFDDP(problem)
+ #solver = crocoddyl.SolverIpopt(problem)
+ # TODO: remove / place elsewhere once it works
+ solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()])
# run solve
- solver.solve()
+ # NOTE: there are some possible parameters here that I'm not using
+ xs = [x0] * (solver.problem.T + 1)
+ us = solver.problem.quasiStatic([x0] * solver.problem.T)
+ solver.solve(xs, us, 5000, False, 1e-9)
+ #solver.solve()
# get reference (state trajectory)
# we aren't using controls because we only have velocity inputs
xs = np.array(solver.xs)
qs = xs[:, :robot.model.nq]
vs = xs[:, robot.model.nq:]
reference = {'qs' : qs, 'vs' : vs, 'dt' : args.ocp_dt}
- return reference
+ return reference, solver
if __name__ == "__main__":
parser = getMinimalArgParser()
parser = get_OCP_args(parser)
args = parser.parse_args()
+ ex_robot = example_robot_data.load("ur5_gripper")
robot = RobotManager(args)
+ # TODO: remove once things work
+ robot.max_qd = 3.14
+ robot.q = pin.randomConfiguration(robot.model)
goal = pin.SE3.Random()
goal.translation = np.random.random(3) * 0.4
print("set goal:", goal)
- reference = IKOCP(args, robot, goal)
+ reference, solver = IKOCP(args, robot, goal)
+ # we only work within -pi - pi (or 0-2pi idk anymore)
+ #reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
+ log = solver.getCallbacks()[1]
+ crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+ crocoddyl.plotConvergence(log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps, figIndex=2)
jointTrajFollowingPD(args, robot, reference)
+ reference.pop('dt')
+ plotFromDict(reference, args.n_knots + 1, args)
print("achieved result", robot.getT_w_e())
+ display = crocoddyl.MeshcatDisplay(ex_robot)
+ display.rate = -1
+ display.freq = 1
+ while True:
+ display.displayFromSolver(solver)
+ time.sleep(1.0)