diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 103355a4a0ad6ca8d169a4e785360471b3cb379b..08d59cf3f1924f6f3aa1ea02dbc239b562c2246c 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -5,14 +5,15 @@ 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 createCrocoEEPathFollowingOCP, solveCrocoOCP
-from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoEndEffectorPathFollowingMPCControlLoop, CrocoEndEffectorPathFollowingMPC, BaseAndEEPathFollowingMPC
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import *
+from ur_simple_control.optimal_control.crocoddyl_mpc import *
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
-from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse
import pinocchio as pin
import crocoddyl
+from functools import partial
import importlib.util
from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
import yaml
@@ -26,7 +27,8 @@ def get_args():
parser = get_OCP_args(parser)
parser = getClikArgs(parser) # literally just for goal error
parser = getPlanningArgs(parser)
- parser.add_argument('--handlebar-height', type=float, default=0.5)
+ parser.add_argument('--handlebar-height', type=float, default=0.5,\
+ help="heigh of handlebar of the cart to be pulled")
parser.add_argument('--base-to-handlebar-preferred-distance', type=float, default=0.7, \
help="prefered path arclength from mobile base position to handlebar")
argcomplete.autocomplete(parser)
@@ -43,6 +45,127 @@ def get_args():
args.n_pol = mpc_params['n_pol']
return args
+def isGraspOK(args, robot, grasp_pose):
+ isOK = False
+ SEerror = robot.getT_w_e().actInv(grasp_pose)
+ err_vector = pin.log6(SEerror).vector
+ # TODO: figure this out
+ # it seems you have to use just the arm to get to finish with this precision
+ #if np.linalg.norm(err_vector) < robot.args.goal_error:
+ if np.linalg.norm(err_vector) < 2*1e-1:
+ isOK = True
+ return not isOK
+
+def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
+ path_planner : ProcessManager, i : int, past_data):
+ """
+ cartPulling
+ 0) if obstacles present, don't move
+ 1) if no obstacles, but not grasped/grasp is off-target, grasp the handlebar with a p2p strategy.
+ 2) if no obstacles, and grasp ok, then pull toward goal with cartPulling mpc
+ 3) parking?
+ 4) ? always true (or do nothing is always true, whatever)
+ """
+ # we use binary as string representation (i don't want to deal with python's binary representation).
+ # the reason for this is that then we don't have disgusting nested ifs
+ priority_register = ['0','1','1']
+ # TODO implement this based on laser scanning or whatever
+ #priority_register[0] = str(int(areObstaclesTooClose()))
+ # TODO: get grasp pose from vision, this is initial hack to see movement
+ if i < 300:
+ grasp_pose = pin.SE3(np.eye(3), np.array([9.0 + args.base_to_handlebar_preferred_distance, 4.0, args.handlebar_height]))
+ else:
+ grasp_pose = robot.getT_w_e()
+ priority_register[1] = str(int(isGraspOK(args, robot, grasp_pose)))
+ # interpret string as base 2 number, return int in base 10
+ priority_int = ""
+ for prio in priority_register:
+ priority_int += prio
+ priority_int = int(priority_int, 2)
+ breakFlag = False
+ save_past_item = {}
+ log_item = {}
+
+ # case 0)
+ if priority_int >= 4:
+ robot.sendQd(np.zeros(robot.model.nv))
+
+ # case 1)
+ # TODO: make it an argument obviously
+ usempc = False
+ if (priority_int < 4) and (priority_int >= 2):
+ # TODO: make goal an argument, remove Mgoal from robot
+ robot.Mgoal = grasp_pose
+ if usempc:
+ # TODO: make it not hit the handlebar or other shit in the process
+ for i, runningModel in enumerate(solver_grasp.problem.runningModels):
+ runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+ solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+ robot.Mgoal = grasp_pose
+ #breakFlag, save_past_item, log_item = CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
+ CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
+ else:
+ #controlLoopClik(robot, invKinmQP, i, past_data)
+ clikController = partial(dampedPseudoinverse, 1e-3)
+ controlLoopClik(robot, clikController, i, past_data)
+
+ # case 2)
+ # MASSIVE TODO:
+ # WHEN STARTING, TO INITIALIZE PREPOPULATE THE PATH WITH AN INTERPOLATION OF
+ # A LINE FROM WHERE THE GRIPPER IS NOW TO THE BASE
+ if priority_int < 2:
+ breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
+ #BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
+
+ q = robot.getQ()
+ p = q[:2]
+ # needed for cart pulling
+ save_past_item['path2D_untimed'] = p
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
+
+
+def cartPulling(args, robot : RobotManager, goal, path_planner):
+ ############################
+ # setup cart-pulling mpc #
+ ############################
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ problem_pulling = createBaseAndEEPathFollowingOCP(args, robot, x0)
+ if args.solver == "boxfddp":
+ solver_pulling = crocoddyl.SolverBoxFDDP(problem_pulling)
+ if args.solver == "csqp":
+ solver_pulling = mim_solvers.SolverCSQP(problem_pulling)
+ xs_init = [x0] * (solver_pulling.problem.T + 1)
+ us_init = solver_pulling.problem.quasiStatic([x0] * solver_pulling.problem.T)
+ solver_pulling.solve(xs_init, us_init, args.max_solver_iter)
+
+ #############################################
+ # setup point-to-point handlebar grasping #
+ # TODO: have option to swith this for clik #
+ #############################################
+ grasp_pose = robot.getT_w_e()
+ problem_grasp = createCrocoIKOCP(args, robot, x0, grasp_pose)
+ if args.solver == "boxfddp":
+ solver_grasp = crocoddyl.SolverBoxFDDP(problem_grasp)
+ if args.solver == "csqp":
+ solver_grasp = mim_solvers.SolverCSQP(problem_grasp)
+ xs_init = [x0] * (solver_grasp.problem.T + 1)
+ us_init = solver_grasp.problem.quasiStatic([x0] * solver_grasp.problem.T)
+ solver_grasp.solve(xs_init, us_init, args.max_solver_iter)
+
+ controlLoop = partial(cartPullingControlLoop, args, robot, goal, solver_grasp, solver_pulling, path_planner)
+
+ log_item = {}
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ save_past_item = {'path2D_untimed' : T_w_e.translation[:2]}
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_item, log_item)
+ loop_manager.run()
+
+
if __name__ == "__main__":
args = get_args()
@@ -51,7 +174,6 @@ if __name__ == "__main__":
robot = RobotManager(args)
robot.q[0] = 9.0
robot.q[1] = 4.0
- #time.sleep(5)
x0 = np.concatenate([robot.getQ(), robot.getQd()])
robot._step()
@@ -79,7 +201,9 @@ if __name__ == "__main__":
planning_function = partial(starPlanner, goal)
# TODO: ensure alignment in orientation between planner and actual robot
path_planner = ProcessManager(args, planning_function, robot.q[:2], 3, None)
- #time.sleep(5)
+ # wait for meshcat to initialize
+ if args.visualize_manipulator:
+ time.sleep(5)
# clik version
#cartesianPathFollowingWithPlanner(args, robot, path_planner)
# end-effector tracking version
@@ -88,9 +212,10 @@ if __name__ == "__main__":
# 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)
+ cartPulling(args, robot, goal, path_planner)
print("final position:")
print(robot.getT_w_e())
+ path_planner.terminateProcess()
if args.save_log:
robot.log_manager.plotAllControlLoops()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 0343be7262ccf487703d207bbfbaeb2cdb691335..e02e89e5479c52cad283376b57866178a3e4284e 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 e1f51ccc304659f0c256d15a324b24fc9c0a6ee0..5f7f84bdf9b600f8d0c911ebd0919c211ea1c011 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/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 7b5300522657104aaccdaa6e489ab6cfc84ee426..c26e75c9ffe7dc58398453c5816f08b85e6f88ef 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -92,9 +92,8 @@ def invKinmQP(J, err_vector, lb=None, ub=None):
# TODO: why is q all 0?
q = np.array([0] * J.shape[1], dtype="double")
G = None
- # TODO: extend for orientation as well
- b = err_vector#[:3]
- A = J#[:3]
+ b = err_vector
+ A = J
# TODO: you probably want limits here
#lb = None
#ub = None
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index f6731d07ba20184911cde95e4cb80c7a95727034..70e8783c25401084bb70c7741593b4353053d1bb 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -88,8 +88,10 @@ def getMinimalArgParser():
default="192.168.1.102")
parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction, \
help="whether you want to just integrate with pinocchio", default=True)
+ parser.add_argument('--ctrl-freq', type=int, \
+ help="frequency of the control loop", default=500)
parser.add_argument('--fast-simulation', action=argparse.BooleanOptionalAction, \
- help="do you want simulation to run over 500Hz? (real-time viz relies on 500Hz)", default=False)
+ help="do you want simulation to as fast as possible? (real-time viz relies on 500Hz)", default=False)
parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction, \
help="whether you want to visualize the manipulator and workspace with meshcat", default=True)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction, \
@@ -440,7 +442,8 @@ class RobotManager:
#self.JOINT_ID = 6
self.ee_frame_id = self.model.getFrameId("tool0")
#self.ee_frame_id = self.model.getFrameId("hande_right_finger_joint")
- self.update_rate = 500 #Hz
+ # TODO: add -1 option here meaning as fast as possible
+ self.update_rate = args.ctrl_freq #Hz
self.dt = 1 / self.update_rate
# you better not give me crazy stuff
# and i'm not clipping it, you're fixing it
@@ -1135,7 +1138,8 @@ class ProcessManager:
self.shm_cmd.unlink()
if self.args.debug_prints:
print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
- self.command_queue.put_nowait("befree")
+ if self.comm_direction != 3:
+ self.command_queue.put_nowait("befree")
try:
self.side_process.terminate()
if self.args.debug_prints:
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index d0a17c5b87a81e5fe305179912ed06bf62197bb5..644615345770826ac408fc3975adc19eebec6fdc 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,6 +1,6 @@
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
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
+from ur_simple_control.path_generation.planner import path2D_timed, pathPointFromPathParam, path2D_to_SE3
import pinocchio as pin
import crocoddyl
import numpy as np
@@ -54,13 +54,13 @@ def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data):
robot.sendQd(vel_cmds)
log_item['qs'] = x0[:robot.model.nq]
- log_item['dqs'] = x0[robot.model.nq:]
+ log_item['dqs'] = x0[robot.model.nv:]
log_item['dqs_cmd'] = vel_cmds
log_item['u_tau'] = us[0]
return breakFlag, save_past_dict, log_item
-def CrocoIKMPC(args, robot, x0, goal):
+def CrocoIKMPC(args, robot, goal):
"""
IKMPC
-----
@@ -69,6 +69,7 @@ def CrocoIKMPC(args, robot, x0, goal):
a dynamics level, and velocities we command
are actually extracted from the state x(q,dq)
"""
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
problem = createCrocoIKOCP(args, robot, x0, goal)
if args.solver == "boxfddp":
solver = crocoddyl.SolverBoxFDDP(problem)
@@ -78,7 +79,6 @@ def CrocoIKMPC(args, robot, x0, goal):
# 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)
@@ -112,6 +112,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
T_w_e = robot.getT_w_e()
p = T_w_e.translation[:2]
+ # TODO: replace with shm
if not (type(path_planner) == types.FunctionType):
path_planner.sendFreshestCommand({'p' : p})
@@ -124,7 +125,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
data = path_planner.getData()
if data == None:
if args.debug_prints:
- print("got no path so no doing anything")
+ print("CTRL: got no path so not i won't move")
robot.sendQd(np.zeros(robot.model.nv))
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
@@ -203,7 +204,7 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
loop_manager.run()
-def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
+def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, iter_n, past_data):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
@@ -219,54 +220,58 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
q = robot.getQ()
T_w_e = robot.getT_w_e()
- #p = T_w_e.translation[:2]
p = q[:2]
- #print("CTRL:", p)
# NOTE: this is the actual position, not what the path suggested
# whether this or path reference should be put in is debateable.
# this feels more correct to me.
save_past_dict['path2D_untimed'] = p
+ path_planner.sendCommandViaSharedMemory(p)
- if not (type(path_planner) == types.FunctionType):
- #path_planner.sendFreshestCommand({'p' : p})
- path_planner.sendCommandViaSharedMemory(p)
-
+ ###########################
+ # get path from planner #
+ ###########################
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
- # get the path from the base from the current base position onward
- 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_base, path2D_untimed_base = data
- #path2D_untimed_base.insert(0, p[1])
- #path2D_untimed_base.insert(0, p[0])
- path2D_untimed_base = np.array(path2D_untimed_base).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
- # TODO: USE THIS ONCE IMPLEMENTED
- #pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol_base, path2D_untimed_base, max_base_v)
- # TODO: separate out timing construction so that it can be used for the handlebar path as well
- pathSE3_base = path2D_to_timed_SE3(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
- # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
- if args.visualize_manipulator:
- if i % 20 == 0:
- robot.visualizer_manager.sendCommand({"path": pathSE3_base})
+ # get the path from the base from the current base position onward.
+ # but we're still fast so who cares
+ data = path_planner.getData()
+ if data == None:
+ if args.debug_prints:
+ print("CTRL: got no path so not i won't move")
+ 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
+ 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
+
+ ##########################################
+ # construct timed 2D path for the base #
+ ##########################################
+ path_pol_base, path2D_untimed_base = data
+ path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
+ # ideally should be precomputed somewhere
+ max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+ # base just needs timing on the path,
+ # and it's of height 0 (i.e. the height of base's planar joint)
+ path_base = path2D_timed(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
- # now find the previous path point of arclength base_to_handlebar_preferred_distance
+ ###################################################
+ # construct timed SE3 path for the end-effector #
+ ###################################################
+ # this works as follow
+ # 1) find the previous path point of arclength base_to_handlebar_preferred_distance.
+ # first part of the path is from there to current base position,
+ # second is just the current base's plan.
+ # 2) construct timing on the first part.
+ # 3) join that with the already timed second part.
+ # 4) turn the timed 2D path into an SE3 trajectory
+
# NOTE: this can be O(1) instead of O(n) but i can't be bothered
path_arclength = np.linalg.norm(p - past_data['path2D_untimed'])
handlebar_path_index = -1
@@ -276,11 +281,33 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
break
path_arclength += np.linalg.norm(past_data['path2D_untimed'][i - 1] - past_data['path2D_untimed'][i])
# i shouldn't need to copy-paste everything but what can you do
- arra = np.array(past_data['path2D_untimed'])
- path2D_handlebar = np.vstack((arra[i:], path2D_untimed_base))
- # now we need to construct timing for this
-
-
+ path2D_handlebar_1_untimed = np.array(past_data['path2D_untimed'])
+ # NOTE: BIG ASSUMPTION
+ # let's say we're computing on time, and that's the time spacing
+ # of previous path points.
+ # this means you need to lower the control frequency argument
+ # if you're not meeting deadlines.
+ # if you really need timing information, you should actually
+ # get it from ControlLoopManager instead of i,
+ # but let's say this is better because you're forced to know
+ # how fast you are instead of ducktaping around delays.
+ # TODO: actually save timing, pass t instead of i to controlLoops
+ # from controlLoopManager
+ # NOTE: this might not working when rosified so check that first
+ time_past = np.linspace(0.0, args.past_window_size * robot.dt, args.past_window_size)
+ s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.n_knots)
+ path2D_handlebar_1 = np.hstack((
+ np.interp(s, time_past, path2D_handlebar_1_untimed[:,0]).reshape((-1,1)),
+ np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
+
+ # TODO: combine with base for maximum correctness
+ pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ # TODO: make it work with just translations too
+ if args.visualize_manipulator:
+ if iter_n % 20 == 0:
+ #robot.visualizer_manager.sendCommand({"path": path_base})
+ robot.visualizer_manager.sendCommand({"path": pathSE3_handlebar})
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
@@ -290,10 +317,12 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
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_base[i]
+ runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
+ runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
# idk if that's necessary
- solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3_base[-1]
+ solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
+ solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
@@ -306,7 +335,7 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
-def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
+def BaseAndEEPathFollowingMPC(args, robot, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
-----
@@ -316,16 +345,13 @@ def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
are actually extracted from the state x(q,dq).
"""
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
problem = createBaseAndEEPathFollowingOCP(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)
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 e323d5969a5b1bb2678bdd21c44996bca3a9c33c..09157b30ead2d179ff9301f477cbc1fb5dec347a 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -396,7 +396,10 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
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
+ # TODO there has to be a path for the base and
+ # a different one for the ee
+ path_ee = [T_w_e] * args.n_knots
+ path_base = [np.append(x0[:2], 0.0)] * 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)
@@ -486,23 +489,21 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
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(
+ eePoseResidual = 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)
+ eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, 1e2)
+ baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
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],
+ path_base[i],
state.nv)
- goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
- runningCostModel.addCost("gripperPose" + str(i), goalTrackingCost, 1e2)
+ baseTrackingCost = crocoddyl.CostModelResidual(state, baseTranslationResidual)
+ runningCostModel.addCost("base_translation" + str(i), baseTrackingCost, 1e2)
if args.solver == "boxfddp":
runningModel = crocoddyl.IntegratedActionModelEuler(
@@ -540,7 +541,8 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
0.0,
)
- terminalCostModel.addCost("gripperPose" + str(args.n_knots), goalTrackingCost, 1e2)
+ terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, 1e2)
+ terminalCostModel.addCost("base_translation" + str(args.n_knots), baseTrackingCost, 1e2)
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index a93d6d84bc2ae6a3181310847a53d17edf420254..f48dca2fca2573f98152a4dfae84776c88ba14a6 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -463,14 +463,12 @@ def pathVisualizer(x0, goal, map_as_list, positions):
def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
-def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height):
+def path2D_timed(args, path_pol, path2D_untimed, max_base_v, path_height):
"""
- path2D_to_timed_SE3
+ path2D_timed
---------------------
- we have a 2D path of (N,2) shape as reference
- the OCP accepts SE3 (it could just rotations or translation too),
- so this function constructs it from the 2D path.
- it also times it as this is what the current ocp formulation needs:
+ we have a 2D path of (N,2) shape as reference.
+ it times it as this is what the current ocp formulation needs:
there should be a timing associated with the path,
because defining the cost function as the fit of the rollout to the path
is complicated to do software-wise.
@@ -487,6 +485,11 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height)
trajectory here, it's a man-made heuristic,
and we should leave that to the MPC,
but that requires too much coding and there is no time rn.
+ the idea is to use compute the tangent of the path,
+ and use that to make a 2D frenet frame.
+ this is the put to some height, making it SE3.
+ i.e. roll and pitch are fixed to be 0,
+ but you could make them some other constant
"""
####################################################
@@ -530,28 +533,24 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height)
# what it should be but gets stuck if we're not yet on path
#s = (i * args.ocp_dt) * t_to_s
# full path
+ # NOTE: this should be wrong, and ocp_dt correct,
+ # but it works better for some reason xd
s = i * (max_s / args.n_knots)
#path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
path2D.append(np.array([np.interp(s, s_vec, path2D_untimed[:,0]),
- np.interp(s, s_vec, path2D_untimed[:,1])]))
+ np.interp(s, s_vec, path2D_untimed[:,1]),
+ path_height]))
path2D = np.array(path2D)
+ return path2D
- ####################################################
- # 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)
-
-
+def path2D_to_SE3(path2D, path_height):
+ """
+ path2D_SE3
+ ----------
+ we have a 2D path of (N,2) shape as reference
+ the OCP accepts SE3 (it could just rotations or translation too),
+ so this function constructs it from the 2D path.
+ """
#########################
# path2D into pathSE2 #
#########################
@@ -566,170 +565,36 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height)
# 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 = []
+ pathSE3 = []
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], path_height])
- path.append(pin.SE3(rotation, translation))
-
- return path
+ pathSE3.append(pin.SE3(rotation, translation))
+ pathSE3.append(pin.SE3(rotation, translation))
+ return pathSE3
+def path2D_to_timed_SE3(todo):
+ pass
-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))
- 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
- # TODO: we need a function that takes time as input and spits
-# 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 = (args.base_to_handlebar_preferred_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))
-
- return path
-
-# function to be put into ProcessManager,
-# spitting out path points
-#def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
-# let's try shared memory
def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, shm_data):
"""
starPlanner
------------
- wild dark dynamical system magic done by albin,
- with more dark magic on top to get it to spit
+ function to be put into ProcessManager,
+ spitting out path points.
+ it's wild dark dynamical system magic done by albin,
+ with software dark magic on top to get it to spit
out path points and just the path points.
- goal and path are [x,y]
+ goal and path are [x,y],
+ but there are utility functions to construct SE3 paths out of this
+ elsewhere in the library.
"""
# shm stuff
shm = shared_memory.SharedMemory(name=shm_name)
@@ -764,12 +629,19 @@ def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, shm_data):
# has to be a blocking call
#cmd = cmd_queue.get()
#p = cmd['p']
+ # TODO: make a sendCommand type thing which will
+ # handle locking, pickling or numpy-arraying for you
+ # if for no other reason than not copy-pasting code
lock.acquire()
p[:] = p_shared[:]
lock.release()
if np.linalg.norm(p - goal) < convergence_threshold:
- data_queue.put("done")
+ data_pickled = pickle.dumps("done")
+ lock.acquire()
+ shm_data.buf[:len(data_pickled)] = data_pickled
+ #shm_data.buf[len(data_pickled):] = bytes(0)
+ lock.release()
break
# Update the scene