diff --git a/examples/cart_pulling/cart_pulling.py b/examples/cart_pulling/cart_pulling.py index d54a4bffeb7396a2cd765f7bd7ff07df7bd77489..647dac9bb3ad731c32cba7ae8b2fc4ef893e435c 100644 --- a/examples/cart_pulling/cart_pulling.py +++ b/examples/cart_pulling/cart_pulling.py @@ -8,7 +8,7 @@ from smc.control.cartesian_space import ( getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, - invKinmQP, + QPquadprog, dampedPseudoinverse, controlLoopClikArmOnly, controlLoopClikDualArmsOnly, diff --git a/examples/cart_pulling/todos.toml b/examples/cart_pulling/todos.toml index f47abe574573a280f5b89cfdeeca1584ec810cc1..685bd60eda1049799be7e282450da2ebd6721828 100644 --- a/examples/cart_pulling/todos.toml +++ b/examples/cart_pulling/todos.toml @@ -551,6 +551,13 @@ assignee = "Marko" name = "dual arm p2p and path following" description = """ refactor end-effector path following + +1. p2p dual arm only mpc +2. p2p dual arm and base +3. fixed path following dual arm and base +4. path following no planner just dual arm reference on mobile yumi +5. path following from planner just dual arm on mobile yumi +6. path following from planner dual arm and base """ priority = 1 is_done = false @@ -559,7 +566,7 @@ dependencies = ["dual arm feature", "refactor end-effector path following"] purpose = """ half of paper problem """ -hours_required = 3 +hours_required = 10 assignee = "Marko" [[project_plan.action_items.action_items.action_items]] diff --git a/examples/cartesian_space/test_by_running.sh b/examples/cartesian_space/test_by_running.sh index 61908aca2f3881cb6e3f9504a2ece88e352eb45e..79f103c6ffbe76d66bd89b36c7e960a9bfc3fe25 100755 --- a/examples/cartesian_space/test_by_running.sh +++ b/examples/cartesian_space/test_by_running.sh @@ -1,47 +1,75 @@ #!/bin/bash # the idea here is to run all the runnable things # and test for syntax errors - -################### -# classic cliks # -################### -# damped pseudoinverse arm +# +# ################ +# single arm +# ############### +# damped pseudoinverse runnable="clik_point_to_point.py --randomly-generate-goal --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2000" echo $runnable python $runnable -# QP arm -runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +# QP quadprog +runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +echo $runnable +python $runnable + +# QP proxsuite +runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable -# damped pseudoinverse whole body single arm mobile +###################### +# whole body single arm +###################### +# damped pseudoinverse runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable +# QP quadprog +runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +echo $runnable +python $runnable -# QP whole body single arm + mobile -runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +# QP proxsuite +runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable -# dual arm pseudoinverse +###################### +# dual arm +# #################### +# damped pseudoinverse runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable -# dual arm QP -runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +# QP quadprog +runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable -# damped pseudoinverse whole body dual arm + mobile +# QP proxsuite +runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +echo $runnable +python $runnable + +# ########################## +# whole body dual arm +# ########################## +# damped pseudoinverse runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable -# QP whole body dual arm + mobile -runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +# QP quadprog +runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" +echo $runnable +python $runnable + +# QP proxsuite +runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000" echo $runnable python $runnable diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py index 524ce0a06ef5d1b942493572a7cf1156e457f866..f50ae8319133e851335646956676177938370a2d 100644 --- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py +++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py @@ -23,7 +23,7 @@ def controlLoopClik( T_w_goal: pin.SE3, args: Namespace, robot: SingleArmInterface, - i: int, + t: int, past_data: dict[str, deque[np.ndarray]], ) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]: """ @@ -41,8 +41,14 @@ def controlLoopClik( # qd = ik_solver(J, err_vector, past_qd=past_data['dqs_cmd'][-1]) v_cmd = ik_solver(J, err_vector) if v_cmd is None: - print("the controller you chose didn't work, using dampedPseudoinverse instead") + print( + t, + "the controller you chose produced None as output, using dampedPseudoinverse instead", + ) v_cmd = dampedPseudoinverse(1e-2, J, err_vector) + else: + if args.debug_prints: + print(t, "ik solver success") return v_cmd, {}, {} diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py index 464f26e3cac14690eb5be2306411fee023500085..60d054c1f4606e4ae6e119351b9b734c8e43ce92 100644 --- a/python/smc/control/cartesian_space/ik_solvers.py +++ b/python/smc/control/cartesian_space/ik_solvers.py @@ -36,13 +36,10 @@ def getIKSolver( # return invKinm_PseudoInv # if controller_name == "invKinm_PseudoInv_half": # return invKinm_PseudoInv_half - if args.ik_solver == "invKinmQP": - lb = -1 * np.array(robot.model.velocityLimit, dtype="double") - # we do additional clipping - lb = np.clip(lb, -1 * robot._max_v, robot._max_v) - ub = np.array(robot.model.velocityLimit, dtype="double") - ub = np.clip(ub, -1 * robot._max_v, robot._max_v) - return partial(invKinmQP, lb=lb, ub=ub) + if args.ik_solver == "QPquadprog": + lb = -1 * robot._max_v.copy() + ub = robot._max_v.copy() + return partial(QPquadprog, lb=lb, ub=ub) if args.ik_solver == "QPproxsuite": H = np.eye(robot.nv) g = np.zeros(robot.nv) @@ -56,7 +53,7 @@ def getIKSolver( qp = proxqp.dense.QP(robot.model.nv, 6, robot.model.nv) qp.init(H, g, A, b, G, lb, ub) qp.solve() - return partial(QPproxsuite, qp, lb, ub) + return partial(QPproxsuite, qp) # if controller_name == "invKinmQPSingAvoidE_kI": # return invKinmQPSingAvoidE_kI @@ -88,11 +85,11 @@ def jacobianTranspose(J: np.ndarray, err_vector: np.ndarray) -> np.ndarray: # TODO: put something into q of the QP # also, put in lb and ub # this one is with qpsolvers -def invKinmQP( - J: np.ndarray, err_vector: np.ndarray, lb=None, ub=None, past_qd=None +def QPquadprog( + J: np.ndarray, V_e_e: np.ndarray, lb=None, ub=None, past_qd=None ) -> np.ndarray: """ - invKinmQP + QPquadprog --------- generic QP: minimize 1/2 x^T P x + q^T x @@ -113,7 +110,25 @@ def invKinmQP( # we set it to 0 here, but we should give a sane default here q = np.array([0] * J.shape[1], dtype="double") G = None - b = err_vector + # NOTE: if err_vector is too big, J * qd = err_vector is infeasible + # for the given ub, lb! (which makes perfect sense as it makes the constraints + # incompatible) + # thus we have to scale it to the maximum followable value under the given + # inequality constraints on the velocities. + # TODO: + # unfortunatelly we need to do some non-trivial math to figure out what the fastest + # possible end-effector velocity in the V_e_e direction is possible + # given our current bounds. this is because it depends on the configuration, + # i.e. on J of course - if we are in a singularity, then maximum velocity is 0! + # and otherwise it's proportional to the manipulability ellipsoid. + # NOTE: for now we're just eyeballing for sport + # NOTE: this fails in low manipulability regions (which makes perfect sense also) + V_e_e_norm = np.linalg.norm(V_e_e) + max_V_e_e_norm = 0.3 + if V_e_e_norm < max_V_e_e_norm: + b = V_e_e + else: + b = (V_e_e / V_e_e_norm) * max_V_e_e_norm A = J # TODO: you probably want limits here # lb = None @@ -145,15 +160,33 @@ def invKinmQP( def QPproxsuite( qp: proxqp.dense.QP, - lb: np.ndarray, - ub: np.ndarray, J: np.ndarray, - err_vector: np.ndarray, + V_e_e: np.ndarray, ) -> np.ndarray: # proxqp does lb <= Cx <= ub qp.settings.initial_guess = proxqp.InitialGuess.WARM_START_WITH_PREVIOUS_RESULT # qp.update(g=q, A=A, b=h, l=lb, u=ub) - qp.update(A=J, b=err_vector) + # NOTE: if err_vector is too big, J * qd = err_vector is infeasible + # for the given ub, lb! (which makes perfect sense as it makes the constraints + # incompatible) + # thus we have to scale it to the maximum followable value under the given + # inequality constraints on the velocities. + # TODO: + # unfortunatelly we need to do some non-trivial math to figure out what the fastest + # possible end-effector velocity in the V_e_e direction is possible + # given our current bounds. this is because it depends on the configuration, + # i.e. on J of course - if we are in a singularity, then maximum velocity is 0! + # and otherwise it's proportional to the manipulability ellipsoid. + # NOTE: for now we're just eyeballing for sport + # NOTE: this fails in low manipulability regions (which makes perfect sense also) + V_e_e_norm = np.linalg.norm(V_e_e) + max_V_e_e_norm = 0.3 + if V_e_e_norm < max_V_e_e_norm: + b = V_e_e + else: + b = (V_e_e / V_e_e_norm) * max_V_e_e_norm + # qp.update(A=J, b=err_vector) + qp.update(A=J, b=b) qp.solve() qd = qp.results.x diff --git a/python/smc/control/cartesian_space/utils.py b/python/smc/control/cartesian_space/utils.py index f684489497e1976d20493acc42a94d7e8aa91de6..4bdbccb15677c19c8ba856d3d3edaa6bb50d34f5 100644 --- a/python/smc/control/cartesian_space/utils.py +++ b/python/smc/control/cartesian_space/utils.py @@ -44,7 +44,7 @@ def getClikArgs(parser: argparse.ArgumentParser): choices=[ "dampedPseudoinverse", "jacobianTranspose", - "invKinmQP", + "QPquadprog", "QPproxsuite", ], )