diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index e702cee34fc5f7a232a48b65c5c8f5cf164e8f9f..9df36a683218250dd71f4f754e4f3340147f1cf2 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -5,8 +5,8 @@ import argparse, argcomplete
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoPathFollowingOCP1, solveCrocoOCP
-from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoPathFollowingMPCControlLoop, CrocoPathFollowingMPC
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoEEPathFollowingOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoEndEffectorPathFollowingMPCControlLoop, CrocoEndEffectorPathFollowingMPC, BaseAndEEPathFollowingMPC
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
@@ -72,10 +72,17 @@ if __name__ == "__main__":
robot.visualizer_manager.sendCommand(command)
planning_function = partial(starPlanner, goal)
+ # TODO: ensure alignment in orientation between planner and actual robot
path_planner = ProcessManager(args, planning_function, None, 2, None)
- cartesianPathFollowingWithPlanner(args, robot, path_planner)
- #CrocoPathFollowingMPC(args, robot, x0, path_planner)
-
+ # clik version
+ #cartesianPathFollowingWithPlanner(args, robot, path_planner)
+ # end-effector tracking version
+ #CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner)
+ # base tracking version (TODO: implement a reference for ee too)
+ # and also make the actual path for the cart and then construct the reference
+ # for the mobile base out of a later part of the path)
+ # atm this is just mobile base tracking
+ BaseAndEEPathFollowingMPC(args, robot, x0, path_planner)
print("final position:")
print(robot.getT_w_e())
diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py
index 8afaf5703d3ee43b8cc1d1bcdd647c990ee26a53..1b2e8098272fa279cacf0278236189e88ab958d5 100644
--- a/python/examples/path_following_mpc.py
+++ b/python/examples/path_following_mpc.py
@@ -5,8 +5,8 @@ import argparse, argcomplete
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoPathFollowingOCP1, solveCrocoOCP
-from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoPathFollowingMPCControlLoop, CrocoPathFollowingMPC
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoEEPathFollowingOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoEndEffectorPathFollowingMPCControlLoop, CrocoEndEffectorPathFollowingMPC
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
@@ -15,6 +15,20 @@ import pinocchio as pin
import crocoddyl
import importlib.util
+def path(T_w_e, i):
+ # 2) do T_mobile_base_ee_pos to get
+ # end-effector reference frame(s)
+
+ # generate bullshit just to see it works
+ path = []
+ t = i * robot.dt
+ for i in range(args.n_knots):
+ t += 0.01
+ new = T_w_e.copy()
+ translation = 2 * np.array([np.cos(t/20), np.sin(t/20), 0.3])
+ new.translation = translation
+ path.append(new)
+ return path
def get_args():
parser = getMinimalArgParser()
@@ -30,16 +44,8 @@ if __name__ == "__main__":
if importlib.util.find_spec('mim_solvers'):
import mim_solvers
robot = RobotManager(args)
- time.sleep(5)
- # TODO: put this back for nicer demos
- #Mgoal = robot.defineGoalPointCLI()
- #Mgoal = pin.SE3.Random()
+ #time.sleep(5)
- #robot.Mgoal = Mgoal.copy()
- #if args.visualize_manipulator:
- # # TODO document this somewhere
- # robot.manipulator_visualizer_queue.put(
- # {"Mgoal" : Mgoal})
# create and solve the optimal control problem of
# getting from current to goal end-effector position.
# reference is position and velocity reference (as a dictionary),
@@ -47,28 +53,9 @@ if __name__ == "__main__":
# starting state
x0 = np.concatenate([robot.getQ(), robot.getQd()])
robot._step()
- T_w_e = robot.getT_w_e()
- path = [T_w_e] * args.n_knots
-
- problem = createCrocoPathFollowingOCP1(args, robot, x0, path)
-
- # NOTE: this might be useful if we solve with a large time horizon,
- # lower frequency, and then follow the predicted trajectory with velocity p-control
- # this shouldn't really depend on x0 but i can't be bothered
- #reference, solver = solveCrocoOCP(args, robot, problem, x0)
- #if args.solver == "boxfddp":
- # log = solver.getCallbacks()[1]
- # crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
- #if args.solver == "csqp":
- # log = solver.getCallbacks()[1]
- # mim_solvers.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
-
- # we need a way to follow the reference trajectory,
- # both because there can be disturbances,
- # and because it is sampled at a much lower frequency
- #followKinematicJointTrajP(args, robot, reference)
- CrocoPathFollowingMPC(args, robot, x0, path)
+ problem = createCrocoEEPathFollowingOCP(args, robot, x0)
+ CrocoEndEffectorPathFollowingMPC(args, robot, x0, path)
print("final position:")
print(robot.getT_w_e())
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index fd39d940c984e5b9546f816ca6829f8138fb8e99..8b2e1e5ebef5450829084854b558d13de09d65bb 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/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 3255f1818e5bdf7182b33c8f7779adf790273f80..7b5300522657104aaccdaa6e489ab6cfc84ee426 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -271,7 +271,7 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
q = robot.getQ()
T_w_e = robot.getT_w_e()
- p = q[:2]
+ p = T_w_e.translation[:2]
path_planner.sendFreshestCommand({'p' : p})
# NOTE: it's pointless to recalculate the path every time
@@ -304,16 +304,19 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# NOTE: obviously not path following but i want to see things working here
- SEerror = T_w_e.actInv(pathSE3[1])
+ SEerror = T_w_e.actInv(pathSE3[-1])
err_vector = pin.log6(SEerror).vector
- #vel_cmd = invKinmQP(J, err_vector, lb=-1*robot.model.velocityLimit,
- # ub=robot.model.velocityLimit)
- #vel_cmd = invKinmQP(J, err_vector)
+ lb = -1*robot.model.velocityLimit
+ lb[1] = -0.001
+ ub = robot.model.velocityLimit
+ ub[1] = 0.001
+ #vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
robot.sendQd(vel_cmd)
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ #time.sleep(0.01)
return breakFlag, save_past_dict, log_item
def cartesianPathFollowingWithPlanner(args, robot : RobotManager, path_planner : ProcessManager):
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index c097dd826cde9f24438f1c66eeaace6a8e4bee77..79b63f59d192ad5fe83657c536d722a50d3c3851 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1070,7 +1070,6 @@ class ProcessManager:
worst case the slave gets one cycle old data, but again,
we're not optimizing all the way
"""
- # This literally doesn't work ubt whatever:
if self.command_queue.qsize() < 1:
#self.command_queue.get_nowait()
self.command_queue.put_nowait(command)
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index c85e7daee11dca7ada368d09c067dbf6afe890ee..143ca68edced3eb0942f98f3e38267d5248aa288 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,10 +1,11 @@
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoPathFollowingOCP1
-from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoEEPathFollowingOCP, createBaseAndEEPathFollowingOCP
+from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam, path2D_to_timed_SE3_base_and_ee
import pinocchio as pin
import crocoddyl
import numpy as np
from functools import partial
+import types
import importlib.util
if importlib.util.find_spec('mim_solvers'):
import mim_solvers
@@ -93,64 +94,167 @@ def CrocoIKMPC(args, robot, x0, goal):
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
+def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
+ """
+ CrocoPathFollowingMPCControlLoop
+ -----------------------------
+ end-effector(s) follow their path(s).
+ path planner can either be a function which spits out a list of path points
+ or an instance of ProcessManager which spits out path points
+ by calling ProcessManager.getData()
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ p = T_w_e.translation[:2]
-def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
+ if not (type(path_planner) == types.FunctionType):
+ path_planner.sendFreshestCommand({'p' : p})
+
+ # NOTE: it's pointless to recalculate the path every time
+ # if it's the same 2D path
+
+ if type(path_planner) == types.FunctionType:
+ pathSE3 = path_planner(T_w_e, i)
+ else:
+ data = path_planner.getData()
+ if data == None:
+ if args.debug_prints:
+ print("got no path so no doing anything")
+ robot.sendQd(np.zeros(robot.model.nv))
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+ if data == "done":
+ breakFlag = True
+
+ path_pol, path2D_untimed = data
+ path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ # should be precomputed somewhere but this is nowhere near the biggest problem
+ max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+
+ # 1) make 6D path out of [[x0,y0],...]
+ # that represents the center of the cart
+ pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualize_manipulator:
+ if i % 20 == 0:
+ robot.visualizer_manager.sendCommand({"path": pathSE3})
+
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ solver.problem.x0 = x0
+ # warmstart solver with previous solution
+ xs_init = list(solver.xs[1:]) + [solver.xs[-1]]
+ xs_init[0] = x0
+ us_init = list(solver.us[1:]) + [solver.us[-1]]
+
+ for i, runningModel in enumerate(solver.problem.runningModels):
+ runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3[i]
+
+ # idk if that's necessary
+ solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3[-1]
+
+ solver.solve(xs_init, us_init, args.max_solver_iter)
+ xs = np.array(solver.xs)
+ us = np.array(solver.us)
+ vel_cmds = xs[1, robot.model.nq:]
+
+ robot.sendQd(vel_cmds)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
+ """
+ CrocoEndEffectorPathFollowingMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq).
+ """
+
+ problem = createCrocoEEPathFollowingOCP(args, robot, x0)
+ if args.solver == "boxfddp":
+ solver = crocoddyl.SolverBoxFDDP(problem)
+ if args.solver == "csqp":
+ solver = mim_solvers.SolverCSQP(problem)
+
+ # technically should be done in controlloop because now
+ # it's solved 2 times before the first command,
+ # but we don't have time for details rn
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ xs_init = [x0] * (solver.problem.T + 1)
+ us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
+ solver.solve(xs_init, us_init, args.max_solver_iter)
+
+ controlLoop = partial(CrocoEndEffectorPathFollowingMPCControlLoop, args, robot, solver, path_planner)
+ log_item = {
+ 'qs' : np.zeros(robot.model.nq),
+ 'dqs' : np.zeros(robot.model.nv)
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager.run()
+
+
+def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
- end-effector(s) follow their path(s)
+ end-effector(s) follow their path(s).
+
+ path planner can either be a function which spits out a list of path points
+ or an instance of ProcessManager which spits out path points
+ by calling ProcessManager.getData()
"""
breakFlag = False
log_item = {}
save_past_dict = {}
q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ #p = T_w_e.translation[:2]
p = q[:2]
- path_planner.sendFreshestCommand({'p' : p})
+
+ if not (type(path_planner) == types.FunctionType):
+ path_planner.sendFreshestCommand({'p' : p})
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
- data = path_planner.getData()
- if data == None:
- if args.debug_prints:
- print("got no path so no doing anything")
- robot.sendQd(np.zeros(robot.model.nv))
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
- if data == "done":
- breakFlag = True
- path_pol, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
- # should be precomputed somewhere but this is nowhere near the biggest problem
- max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+ if type(path_planner) == types.FunctionType:
+ pathSE3 = path_planner(T_w_e, i)
+ else:
+ data = path_planner.getData()
+ if data == None:
+ if args.debug_prints:
+ print("got no path so no doing anything")
+ robot.sendQd(np.zeros(robot.model.nv))
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+ if data == "done":
+ breakFlag = True
- # 1) make 6D path out of [[x0,y0],...]
- # that represents the center of the cart
- pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
+ path_pol, path2D_untimed = data
+ path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ # should be precomputed somewhere but this is nowhere near the biggest problem
+ max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+
+ # 1) make 6D path out of [[x0,y0],...]
+ # that represents the center of the cart
+ pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualize_manipulator:
- robot.visualizer_manager.sendCommand({"path": pathSE3})
+ if i % 20 == 0:
+ robot.visualizer_manager.sendCommand({"path": pathSE3})
- # 2) do T_mobile_base_ee_pos to get
- # end-effector reference frame(s)
-
- ##############################################################33
- # generate bullshit just to see it works
- #T_w_e = robot.getT_w_e()
- #path = []
- #t = i * robot.dt
- #for i in range(args.n_knots):
- # t += 0.01
- # new = T_w_e.copy()
- # translation = 2 * np.array([np.cos(t/20), np.sin(t/20), 0.3])
- # new.translation = translation
- # path.append(new)
- ##############################################################33
-
-
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
@@ -175,18 +279,17 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
-
-def CrocoPathFollowingMPC(args, robot, x0, path_planner):
+def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
"""
- IKMPC
+ CrocoEndEffectorPathFollowingMPC
-----
run mpc for a point-to-point inverse kinematics.
note that the actual problem is solved on
a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq)
+ are actually extracted from the state x(q,dq).
"""
- problem = createCrocoPathFollowingOCP1(args, robot, x0)
+ problem = createBaseAndEEPathFollowingOCP(args, robot, x0)
if args.solver == "boxfddp":
solver = crocoddyl.SolverBoxFDDP(problem)
if args.solver == "csqp":
@@ -200,7 +303,7 @@ def CrocoPathFollowingMPC(args, robot, x0, path_planner):
us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
solver.solve(xs_init, us_init, args.max_solver_iter)
- controlLoop = partial(CrocoPathFollowingMPCControlLoop, args, robot, solver, path_planner)
+ controlLoop = partial(BaseAndEEPathFollowingMPCControlLoop, args, robot, solver, path_planner)
log_item = {
'qs' : np.zeros(robot.model.nq),
'dqs' : np.zeros(robot.model.nv)
@@ -208,3 +311,4 @@ def CrocoPathFollowingMPC(args, robot, x0, path_planner):
save_past_dict = {}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
+
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 76b770e02362651d9671ee0d185e7dd7b714a347..e323d5969a5b1bb2678bdd21c44996bca3a9c33c 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -222,33 +222,19 @@ def solveCrocoOCP(args, robot, problem, x0):
-def createCrocoPathFollowingOCP1(args, robot : RobotManager, x0):
+def createCrocoEEPathFollowingOCP(args, robot : RobotManager, x0):
"""
- createCrocoPathFollowingOCP1
+ createCrocoEEPathFollowingOCP
-------------------------------
- creates a path following problem.
+ creates a path following problem with a single end-effector reference.
it is instantiated to just to stay at the current position.
NOTE: the path MUST be time indexed with the SAME time used between the knots
"""
T_w_e = robot.getT_w_e()
path = [T_w_e] * args.n_knots
- # 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.5
- #robot.data = robot.model.createData()
- # TODO: make it underactuated in mobile base's y-direction
+ # underactuation is done by setting max-torque to 0 in the robot model,
+ # and since we torques are constrained we're good
state = crocoddyl.StateMultibody(robot.model)
- # command input IS torque
- # TODO: consider ActuationModelFloatingBaseTpl for heron
- # TODO: create a different actuation model (or whatever)
- # for the mobile base - basically just remove the y movement in the base
- # and update the corresponding derivates to 0
- # there's python examples for this, ex. acrobot.
- # you might want to implement the entire action model too idk what's really necessary here
actuation = crocoddyl.ActuationModelFull(state)
# we will be summing 4 different costs
@@ -294,6 +280,7 @@ def createCrocoPathFollowingOCP1(args, robot : RobotManager, x0):
xub = np.concatenate([
robot.model.upperPositionLimit,
robot.model.velocityLimit])
+
# we have no limits on the mobile base.
# the mobile base is a planar joint.
# since it is represented as [x,y,cos(theta),sin(theta)], there is no point
@@ -306,8 +293,6 @@ def createCrocoPathFollowingOCP1(args, robot : RobotManager, x0):
# from one to point to the other.
# point activation input and the residual need to be of the same length obviously,
# and this should be 2 * model.nv the way things are defined here.
-
-
if robot.robot_name == "heron":
xlb = xlb[1:]
xub = xub[1:]
@@ -322,12 +307,7 @@ def createCrocoPathFollowingOCP1(args, robot : RobotManager, x0):
limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
terminalCostModel.addCost("limitCost", limitCost, 1e3)
- # TODO: try using crocoddyl.ConstraintModelResidual
- # instead to create actual box constraints (inequality constraints)
- # TODO: same goes for control input
- # NOTE: i'm not sure whether any solver uses these tho lel
- # --> you only do that for mim_solvers' csqp!
-
+ # csqp actually allows us to put in hard constraints so we do that
if args.solver == "csqp":
# this just store all the constraints
constraints = crocoddyl.ConstraintModelManager(state, robot.model.nv)
@@ -407,6 +387,165 @@ def createCrocoPathFollowingOCP1(args, robot : RobotManager, x0):
problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
return problem
+def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
+ """
+ createBaseAndEEPathFollowingOCP
+ -------------------------------
+ creates a path following problem.
+ it is instantiated to just to stay at the current position.
+ NOTE: the path MUST be time indexed with the SAME time used between the knots
+ """
+ T_w_e = robot.getT_w_e()
+ path = [T_w_e] * args.n_knots
+ # underactuation is done by setting max-torque to 0 in the robot model,
+ # and since we torques are constrained we're good
+ state = crocoddyl.StateMultibody(robot.model)
+ actuation = crocoddyl.ActuationModelFull(state)
+
+ # we will be summing 6 different costs
+ # first 3 are for tracking, state and control regulation,
+ # the others depend on the path and will be defined later
+ terminalCostModel = crocoddyl.CostModelSum(state)
+ # cost 1) u residual (actuator cost)
+ uResidual = crocoddyl.ResidualModelControl(state, state.nv)
+ uRegCost = crocoddyl.CostModelResidual(state, uResidual)
+ # cost 2) x residual (overall amount of movement)
+ xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+ xRegCost = crocoddyl.CostModelResidual(state, xResidual)
+
+ # put these costs into the running costs
+ # we put this one in later
+ # and add the terminal cost, which is the distance to the goal
+ terminalCostModel.addCost("uReg", uRegCost, 1e3)
+
+ ######################################################################
+ # state constraints #
+ #################################################
+ # - added to costs via barrier functions for fddp (4th cost function)
+ # - added as actual constraints via crocoddyl.constraintmanager for csqp
+ ###########################################################################
+ xlb = np.concatenate([
+ robot.model.lowerPositionLimit,
+ -1 * robot.model.velocityLimit])
+ xub = np.concatenate([
+ robot.model.upperPositionLimit,
+ robot.model.velocityLimit])
+ # we have no limits on the mobile base.
+ # the mobile base is a planar joint.
+ # since it is represented as [x,y,cos(theta),sin(theta)], there is no point
+ # to limiting cos(theta),sin(theta) even if we wanted limits,
+ # because we would then limit theta, or limit ct and st jointly.
+ # in any event, xlb and xub are 1 number too long --
+ # the residual has to be [x,y,theta] because it is in the tangent space -
+ # the difference between two points on a manifold in pinocchio is defined
+ # as the velocity which if parallel transported for 1 unit of "time"
+ # from one to point to the other.
+ # point activation input and the residual need to be of the same length obviously,
+ # and this should be 2 * model.nv the way things are defined here.
+
+
+ if robot.robot_name == "heron":
+ xlb = xlb[1:]
+ xub = xub[1:]
+
+ # TODO: make these constraints-turned-to-objectives for end-effector following
+ # the handlebar position
+ if args.solver == "boxfddp":
+ bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
+ xLimitResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+ xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
+
+ limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
+ terminalCostModel.addCost("limitCost", limitCost, 1e3)
+
+
+ # csqp actually allows us to put in hard constraints so we do that
+ if args.solver == "csqp":
+ # this just store all the constraints
+ constraints = crocoddyl.ConstraintModelManager(state, robot.model.nv)
+ u_constraint = crocoddyl.ConstraintModelResidual(
+ state,
+ uResidual,
+ -1 * robot.model.effortLimit * 0.1,
+ robot.model.effortLimit * 0.1)
+ constraints.addConstraint("u_box_constraint", u_constraint)
+ x_constraint = crocoddyl.ConstraintModelResidual(
+ state,
+ xResidual,
+ xlb,
+ xub)
+ constraints.addConstraint("x_box_constraint", x_constraint)
+
+ # Next, we need to create an action model for running and terminal knots. The
+ # forward dynamics (computed using ABA) are implemented
+ # inside DifferentialActionModelFreeFwdDynamics.
+ runningModels = []
+ for i in range(args.n_knots):
+ runningCostModel = crocoddyl.CostModelSum(state)
+ runningCostModel.addCost("xReg", xRegCost, 1e-3)
+ runningCostModel.addCost("uReg", uRegCost, 1e-3)
+ if args.solver == "boxfddp":
+ runningCostModel.addCost("limitCost", limitCost, 1e3)
+ # TODO: implement
+# framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+# state,
+# robot.model.getFrameId("tool0"),
+# path["ee"][i], # this better be done with the same time steps as the knots
+# # NOTE: this should be done outside of this function to clarity
+# state.nv)
+ framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ robot.model.getFrameId("mobile_base"),
+ # TODO: implement
+ #path["base"][i], # this better be done with the same time steps as the knots
+ # # NOTE: this should be done outside of this function to clarity
+ path[i],
+ state.nv)
+ goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
+ runningCostModel.addCost("gripperPose" + str(i), goalTrackingCost, 1e2)
+
+ if args.solver == "boxfddp":
+ runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, runningCostModel
+ ),
+ args.ocp_dt,
+ )
+ runningModel.u_lb = -1 * robot.model.effortLimit * 0.1
+ runningModel.u_ub = robot.model.effortLimit * 0.1
+ if args.solver == "csqp":
+ runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, runningCostModel, constraints
+ ),
+ args.ocp_dt,
+ )
+ runningModels.append(runningModel)
+
+ # terminal model
+ if args.solver == "boxfddp":
+ terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, terminalCostModel
+ ),
+ 0.0,
+ )
+ terminalModel.u_lb = -1 * robot.model.effortLimit * 0.1
+ terminalModel.u_ub = robot.model.effortLimit * 0.1
+ if args.solver == "csqp":
+ terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, terminalCostModel, constraints
+ ),
+ 0.0,
+ )
+
+ terminalCostModel.addCost("gripperPose" + str(args.n_knots), goalTrackingCost, 1e2)
+
+ # now we define the problem
+ problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
+ return problem
+
if __name__ == "__main__":
parser = getMinimalArgParser()
parser = get_OCP_args(parser)
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 9596d3ccaffb4bf36cd31d9f7c9db5eae4ae4c92..49658b0825e85ed0d25d8844dcd544a0f1b4e285 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -523,6 +523,9 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
s = (i * args.ocp_dt) * t_to_s
path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
path2D = np.array(path2D)
+ #print("=" * 15)
+ #print(path2D_untimed)
+ #print('----------------------------------------')
####################################################
# constructing the SE3 reference from [x,y] path #
@@ -566,9 +569,149 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
path = []
for i in range(len(path2D) - 1):
rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
+ rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
+ #rotation = np.eye(3)
translation = np.array([path2D[i][0], path2D[i][1],
args.handlebar_height])
path.append(pin.SE3(rotation, translation))
+ #print(path[-1].translation)
+
+ return path
+
+
+def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
+ """
+ path2D_to_timed_SE3
+ ---------------------
+ we have a 2D path of (N,2) shape as reference.
+ from this we construct timed SE3 references for the cart, end-effector(s) and the mobile base,
+ in that order. this is done as follows.
+ 0) the path starts at the cart. we align the end-effector and the mobile base to be
+ ON this path as well. to we can follow the path, we need to make the path smooth enough.
+ that is considered a part of the planning problem.
+ 1) when the initial plan is set, the base-endeffector-cart system needs to be aligned with
+ the path. we can ensure AN initial alignment in the beginning when we grasp the cart.
+ furthermore, we can compute the initial path to see what the alignment should be.
+ then we can let either point-to-point clik or mpc align the bodies to this,
+ or let the controller-planner architecture run,
+ BUT then we need to test the stability w.r.t. initial point.
+ 2) the cart is a rigid body, so it doesn't matter which point of it is being referenced.
+ (one point localizes the whole rigid body because it's rigid).
+ we select the point(s) on the handlebar where we will perform the grasp.
+ 3) DUE to rigid grasping, the end-effector(s)'s reference is the same
+ as the one for the handlebar. thus we actually construct only 2 references from the 2D path.
+ the initial point of the path is this one.
+ 4) the path is of some length (it's a variable in the planner). we ensure it is long enough
+ to fit the base on it. this does mean that we can not GUARANTEE we avoid all obstacles,
+ because the path-planning guarantees this only for the initial path point.
+ 5) TODO for extra fun and profit: make an underactuated 2-point model in the path planner.
+ 6) timing is obtained from the maximum velocity of the robot and total path length.
+ this is a stupid heuristic. the time-scaling of the path should be a decision variable in the OCP.
+ 7) TODO for extra fun and profit: make time-scaling of the path a decision variable in the OCP.
+ """
+
+ ####################################################
+ # getting a timed 2D trajectory from a heuristic #
+ ####################################################
+ # the strategy is somewhat reasonable at least:
+ # assume we're moving at 90% max velocity in the base,
+ # and use that.
+ perc_of_max_v = 0.9
+ base_v = perc_of_max_v * max_base_v
+
+ # 1) the length of the path divided by 0.9 * max_vel
+ # gives us the total time of the trajectory,
+ # so we first compute that
+ # easiest possible way to get approximate path length
+ # (yes it should be from the polynomial approximation but that takes time to write)
+ x_i = path2D_untimed[:,0][:-1] # no last element
+ y_i = path2D_untimed[:,1][:-1] # no last element
+ x_i_plus_1 = path2D_untimed[:,0][1:] # no first element
+ y_i_plus_1 = path2D_untimed[:,1][1:] # no first element
+ x_diff = x_i_plus_1 - x_i
+ x_diff = x_diff.reshape((-1,1))
+ y_diff = y_i_plus_1 - y_i
+ y_diff = y_diff.reshape((-1,1))
+ path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
+ print(path_length)
+ total_time = path_length / base_v
+ # 2) we find the correspondence between s (path parameter) and time
+ # TODO: read from where max_s should be 5, but seba checked that it's 5 for some reason
+ max_s = 5
+ t_to_s = max_s / total_time
+ # 3) construct the ocp-timed 2D path for the cart and for the mobile base of the robot
+ # TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
+ # TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
+ # TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
+ # TODO make this an argument
+ base_to_handlebar_prefered_distance = 0.7
+ # TODO: we need a function that takes time as input and spits
+ !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11
+ need to have set interpolation to implement that
+ # out a path point of distance base_to_handlebar_prefered_distance from time 0.
+ # (it's the classic integral for curve length)
+ # cart_to_base_prefered_distance as percentage of path length * size of s (again why the fuck isn't s=1????)
+ handlebar_to_base_as_s = (cart_to_base_prefered_distance / path_length) * max_s
+ path2D_mobile_base = []
+ path2D_cart = []
+ # time is i * args.ocp_dt
+ for i in range(args.n_knots + 1):
+ s_cart = (i * args.ocp_dt) * t_to_s
+ # since we're pulling the robot is in front of the cart
+ s_mobile_base = s_cart + cart_to_base_as_s
+ path2D_cart.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ path2D_cart = np.array(path2D_cart)
+
+
+ ####################################################
+ # constructing the SE3 reference from [x,y] path #
+ ####################################################
+ # 1) we need to add points so that there are args.n_knots of them
+ # the -1 is here because we're finite differencing to get theta,
+ # so we need one extra
+ #path2D = list(path2D)
+ ## in case there's too few
+ #while len(path2D) - 1 < args.n_knots:
+ # path2D.append(path2D[-1])
+ ## in case there's too much
+ #while len(path2D) - 1> args.n_knots:
+ # path2D.pop()
+ #path2D = np.array(path2D)
+
+
+ #########################
+ # path2D into pathSE2 #
+ #########################
+ # construct theta
+ # since it's a pairwise operation it can't be done on the last point
+ x_i = path2D_cart[:,0][:-1] # no last element
+ y_i = path2D_cart[:,1][:-1] # no last element
+ x_i_plus_1 = path2D_cart[:,0][1:] # no first element
+ y_i_plus_1 = path2D_cart[:,1][1:] # no first element
+ x_diff = x_i_plus_1 - x_i
+ y_diff = y_i_plus_1 - y_i
+ # elementwise arctan2
+ thetas = np.arctan2(x_diff, y_diff)
+
+
+ #thetas = thetas.reshape((-1, 1))
+ #path_SE2 = np.hstack((path2D, thetas))
+
+ ######################################
+ # construct SE3 from SE2 reference #
+ ######################################
+ # the plan is parallel to the ground because it's a mobile
+ # manipulation task
+ # TODO: enforce timing, interpolate the path accordingly
+ path = []
+ for i in range(len(path2D_cart) - 1):
+ rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
+ rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
+ #rotation = np.eye(3)
+ translation = np.array([path2D_cart[i][0], path2D_cart[i][1],
+ args.handlebar_height])
+ path.append(pin.SE3(rotation, translation))
+ #print(path[-1].translation)
return path
@@ -613,6 +756,7 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
p = cmd['p']
if np.linalg.norm(p - goal) < convergence_threshold:
+ data_queue.put("done")
break
# Update the scene
@@ -622,8 +766,6 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
data_queue.put((path_pol, path_gen.target_path))
- if args.debug_prints:
- print("put in new plan")
except KeyboardInterrupt:
if args.debug_prints:
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 2e7416863b44cbd0856cb8c0534e2a51249208a6..5f9c8d3e8db06097c501d6e568f9c82e342ef74f 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index c37f00000e7ae5a486d40bb552efe41910f077c0..71fcdc3f7dba9fb6cfec27070b67852c3b068c22 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -193,6 +193,7 @@ def heron_approximation():
# we should immediately set velocity limits.
# there are no position limit by default and that is what we want.
# TODO: put in heron's values
+ # TODO: make these parameters the same as in mpc_params in the planner
model_mobile_base.velocityLimit[0] = 2
model_mobile_base.velocityLimit[1] = 0
model_mobile_base.velocityLimit[2] = 2
@@ -206,8 +207,7 @@ def heron_approximation():
# pretty much random numbers
# TODO: find heron (mir) numbers
- body_inertia = pin.Inertia.FromBox(30, 0.5,
- 0.3, 0.4)
+ body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
# maybe change placement to sth else depending on where its grasped
model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity())
box_shape = fcl.Box(0.5, 0.3, 0.4)
@@ -218,7 +218,7 @@ def heron_approximation():
geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
# have to add the frame manually
- model_mobile_base.addFrame(pin.Frame('base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+ model_mobile_base.addFrame(pin.Frame('mobile_base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
# frame-index should be 1
model, visual_model = pin.appendModel(model_mobile_base, model_arm, geom_model_mobile_base, visual_model_arm, 1, pin.SE3.Identity())
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 0c5b24f2f30666787d02956795b99414a79b25c3..5e23ff210ab7394dfd4a3855298def2a26a4fc03 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ