diff --git a/examples/cart_pulling/technical_report/cart_pulling.log b/examples/cart_pulling/technical_report/cart_pulling.log
index c94d6154b9f3c5d1684906965a791e55fefc1b52..15cc14d968f6762a583cd13d62591af21d5b2bf1 100644
--- a/examples/cart_pulling/technical_report/cart_pulling.log
+++ b/examples/cart_pulling/technical_report/cart_pulling.log
@@ -1,4 +1,4 @@
-This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 26 FEB 2025 10:30
+This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 26 FEB 2025 11:47
entering extended mode
\write18 enabled.
%&-line parsing enabled.
diff --git a/examples/cart_pulling/technical_report/cart_pulling.pdf b/examples/cart_pulling/technical_report/cart_pulling.pdf
index de7c9151127cbd4e479a974680060eb7de0f65c5..9c4ffafbdc6ff237ebdc0e4f72fd896447bdadc1 100644
Binary files a/examples/cart_pulling/technical_report/cart_pulling.pdf and b/examples/cart_pulling/technical_report/cart_pulling.pdf differ
diff --git a/examples/cart_pulling/todos.toml b/examples/cart_pulling/todos.toml
index bc1f78998f64063e1f4582f3a78dd813fd89f4a5..6712381f5935b9103909e69731584cb68f9db981 100644
--- a/examples/cart_pulling/todos.toml
+++ b/examples/cart_pulling/todos.toml
@@ -27,6 +27,7 @@ is_done = true
dependencies = []
purpose = """being able moving heron and yumi with code"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -44,6 +45,7 @@ is_done = true
dependencies = []
purpose = """verify heron interface to be able to rest assuredly"""
hours_required = 2
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "exploring & understanding whole-body-driver"
@@ -62,6 +64,7 @@ is_done = false
dependencies = []
purpose = """being able to move mobile yumi with code"""
hours_required = 12
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -77,6 +80,7 @@ is_done = false
dependencies = ["exploring & understanding whole-body-driver"]
purpose = """being able to move mobile yumi with something other than whole body driver"""
hours_required = 7
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -92,6 +96,7 @@ purpose = """making sure we are getting the best control performance in the sens
of frequency, also ensures the approach isn't fundamentally bad/incorrect (ROS2 qos
profiles etc)"""
hours_required = 4
+assignee = "Marko"
[[project_plan.action_items]]
name = "fast localization"
@@ -105,6 +110,7 @@ dependencies = []
purpose = """localization is required for correctness of any kind of mobile robot movement,
especially for whole-body control"""
hours_required = 0
+assignee = "Seba"
[[project_plan.action_items.action_items]]
@@ -123,6 +129,7 @@ dependencies = []
purpose = """we need some kind of odometry for fast localization, and it's better to use the provided
thing over wasting time hacking a sub-par solution from various topics"""
hours_required = 6
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "testing whether raw odometry + amcl is enough"
@@ -135,6 +142,7 @@ is_done = false
dependencies = ["understanding and testing odometry"]
purpose = """trying to avoid the work of using a kalman filter"""
hours_required = 3
+assignee = "Seba"
[[project_plan.action_items.action_items]]
@@ -151,6 +159,7 @@ is_done = false
dependencies = ["testing whether raw odometry + amcl is enough"]
purpose = """configuring a kf if necessary"""
hours_required = 6
+assignee = "Seba"
[[project_plan.action_items]]
name = "ros2 whatever map into path planning map"
@@ -173,6 +182,7 @@ dependencies = []
purpose = """localization is required for correctness of any kind of mobile robot movement,
especially for whole-body control"""
hours_required = 0
+assignee = "Seba"
[[project_plan.action_items.action_items]]
@@ -189,6 +199,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 4
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "understanding starifiable obstacles"
@@ -201,6 +212,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 4
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "starify static map"
@@ -213,6 +225,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 3
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "starify dynamic obstacles"
@@ -225,6 +238,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 3
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "continuously starify dynamic obstacles"
@@ -237,6 +251,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 5
+assignee = "Seba"
[[project_plan.action_items]]
name = "computer vision stuff"
@@ -256,6 +271,7 @@ dependencies = []
purpose = """required to solve the real problem of a robot pulling a real cart -
it has to see it. also easily usable in other projects if done right codewise."""
hours_required = 0
+assignee = "X"
[[project_plan.action_items.action_items]]
@@ -269,6 +285,7 @@ is_done = false
dependencies = []
purpose = """needed for yolo cross-validation"""
hours_required = 2
+assignee = "X"
[[project_plan.action_items.action_items]]
@@ -282,6 +299,7 @@ is_done = false
dependencies = []
purpose = """required for cross-validation with yolo"""
hours_required = 4
+assignee = "X"
[[project_plan.action_items.action_items]]
name = "fix pose estimation from point cloud"
@@ -296,6 +314,7 @@ is_done = false
dependencies = []
purpose = """last component of core vision stack, required for anything vision"""
hours_required = 10
+assignee = "X"
[[project_plan.action_items.action_items]]
name = "finding a local server to run grounding sam on"
@@ -310,6 +329,7 @@ purpose = """
grounding sam is a very good off-the-shelf solution for computer vision tasks. this should be available literally at any point in time to enable quick experiments
"""
hours_required = 3
+assignee = "X"
[[project_plan.action_items]]
name = "refactor optimal control code"
@@ -324,6 +344,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -339,6 +360,7 @@ purpose = """
at least 100 less lines of copy-pasted code
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "refactor p2p ocp"
@@ -353,6 +375,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "refactor end-effector path following"
@@ -367,6 +390,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "rewrite ee construction from past path to be more efficient"
@@ -383,6 +407,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "write end-effector and base path following"
@@ -408,13 +433,14 @@ control stack is broken. could be the control itself, or trajectories' construct
or there is something fundamentally fucked with changing the plan on the fly, idk. could be that you need to put in some smoothing into it. can't know unless you write tools to test this shit.
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["refactor optimal control code"]
purpose = """
getting any base and ee path tracking to work
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "manage path saving better"
@@ -436,6 +462,7 @@ purpose = """
getting any base and ee path tracking to work
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -451,6 +478,7 @@ purpose = """
getting any base and ee path tracking to work
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "ensure either reference doesn't change rapidly"
@@ -471,6 +499,7 @@ purpose = """
getting any base and ee path tracking to work
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "debug base and ee tracking"
@@ -481,13 +510,14 @@ do just path reconstruction. it should be a live controlloop.p_base should go ar
PLOT EVERY SINGLE STEP! make the plan providing be on a single processor.
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["dual arm feature", "refactor end-effector path following"]
purpose = """
solving the same problem but with a single arm
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "alternative if full-body mpc is not the right tool for the job"
@@ -496,13 +526,14 @@ control just the base with mpc for base tracking.
make the handlebar be defined in a pose relative to the body. run clik on arms to get to a point made up by the path (still need to find corresponding path point) with the orientation being constructed with normal/tangent of the past path. then run p2p on that single goal. update the goal at every time step. an even better idea if getting whole trajectory works, just run trajectory following.
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["dual arm feature", "refactor end-effector path following"]
purpose = """
half of paper problem
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "dual arm p2p and path following"
@@ -517,6 +548,7 @@ purpose = """
half of paper problem
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "cart-pulling (ee and base path following)"
@@ -531,6 +563,7 @@ purpose = """
half of paper problem
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "construct map showcasing my prefered setting (make the labyrinth!!!!!!!!)"
@@ -545,6 +578,7 @@ purpose = """
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items]]
name = "tools for trajectory generation in smc"
@@ -564,6 +598,7 @@ purpose = """
this tool will solve issues with weird trajectories or wasting 20min to write the correct SE3 transformation, which simply persist otherwise and are super annoying to deal with -> it's much better to do geometry on screen than in your head if you need specific numbers on that geometry.
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -572,11 +607,12 @@ description = """
debug existing trajectory generation
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["smc refactor"]
purpose = """creating the base of interactive goal/transform/trajectory design"""
hours_required = 8
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "extend logging to include saving the whole of save-past-dict"
@@ -597,6 +633,7 @@ deadline = 2025-02-14
dependencies = ["smc refactor"]
purpose = """creating the base of interactive goal/transform/trajectory design"""
hours_required = 8
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "use drawing feature from drawing demo to draw a path to be followed"
@@ -611,6 +648,7 @@ deadline = 2025-02-14
dependencies = []
purpose = """make test design for just path planning take 10 seconds"""
hours_required = 10
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "interactive meshcat for setting a transformation"
@@ -627,6 +665,7 @@ deadline = 2025-02-14
dependencies = ["smc refactor"]
purpose = """creating the base of interactive goal/transform/trajectory design"""
hours_required = 10
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -643,6 +682,7 @@ deadline = 2025-02-14
dependencies = ["interactive meshcat for setting a transformation"]
purpose = """making interaction much more usable"""
hours_required = 2
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -658,6 +698,7 @@ deadline = 2025-02-14
dependencies = ["interactive meshcat for setting a transformation"]
purpose = """making visual verification more powerful and easier"""
hours_required = 4
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -673,6 +714,7 @@ deadline = 2025-02-01
dependencies = ["interactive meshcat for setting a transformation"]
purpose = """making tuning and understanding of path planning much easier"""
hours_required = 6
+assignee = "Marko"
[[project_plan.action_items]]
@@ -690,6 +732,7 @@ deadline = 2025-02-01
dependencies = ["tools for trajectory generation in smc"]
purpose = """having the correct ocp formulation for the whole-body mpc controller for pushing and pulling"""
hours_required = 20
+assignee = "Marko"
[[project_plan.action_items]]
@@ -704,6 +747,7 @@ deadline = 2025-02-01
dependencies = []
purpose = """designing a better ocp, testing agaist different options"""
hours_required = 12
+assignee = "Marko"
[[project_plan.action_items]]
@@ -736,6 +780,7 @@ deadline = 2025-02-01
dependencies = []
purpose = """having functional robotic system, not such 1 controller hopefully working as intended"""
hours_required = 8
+assignee = "Marko"
[[project_plan.action_items]]
@@ -763,6 +808,7 @@ purpose = """
scientific perfomance measurement is required for a scientific publication
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -779,6 +825,7 @@ is_done = false
dependencies = []
purpose = """knowing what to measure and why"""
hours_required = 12
+assignee = "all"
[[project_plan.action_items.action_items]]
@@ -802,6 +849,7 @@ is_done = false
dependencies = []
purpose = """getting data"""
hours_required = 18
+assignee = "Marko"
[[project_plan.action_items]]
name = "write research diary"
@@ -812,7 +860,7 @@ this text is also not the paper text. the paper text is a condensed subsection o
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
-is_done = false
+is_done = true
dependencies = []
purpose = """this is the product of the project"""
hours_required = 6
@@ -832,7 +880,7 @@ is_done = false
dependencies = []
purpose = """this is the product of the project"""
hours_required = 0
-assignee = "Seba"
+assignee = "Marko, Seba"
[[project_plan.action_items.action_items]]
name = "setting colors to authors"
@@ -934,6 +982,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "writing related work"
@@ -948,6 +997,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "writing methodology"
@@ -962,6 +1012,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Marko, Seba"
[[project_plan.action_items.action_items.action_items]]
name = "writing modelling"
@@ -974,6 +1025,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items.action_items]]
name = "writing CV, how to grab a cart, small behavior tree mabe"
@@ -986,6 +1038,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "X"
[[project_plan.action_items.action_items.action_items]]
name = "writing path planning"
@@ -998,6 +1051,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items.action_items]]
name = "writing controller"
@@ -1010,6 +1064,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -1025,6 +1080,7 @@ is_done = false
dependencies = ["performing experiments"]
purpose = """part of paper"""
hours_required = 18
+assignee = "Marko"
@@ -1042,3 +1098,4 @@ dependencies = ["writing introdution, related work, methodology",
"writing experiments, discussion"]
purpose = """part of paper"""
hours_required = 24
+assignee = "all"
diff --git a/examples/cartesian_space/dual_arm_clik.py b/examples/cartesian_space/dual_arm_clik.py
new file mode 100644
index 0000000000000000000000000000000000000000..b9cfc1f0c5025f2f0f0f2b8c9fece43a8dce83b5
--- /dev/null
+++ b/examples/cartesian_space/dual_arm_clik.py
@@ -0,0 +1,50 @@
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.util.define_random_goal import getRandomlyGeneratedGoal
+from smc.control.cartesian_space import getClikArgs
+from smc.robots.utils import defineGoalPointCLI
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveLDualArm
+
+import numpy as np
+import argparse
+import pinocchio as pin
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = "Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!"
+ parser = getClikArgs(parser)
+ parser.add_argument(
+ "--randomly-generate-goal",
+ action=argparse.BooleanOptionalAction,
+ default=False,
+ help="if true, rand generate a goal, if false you type it in via text input",
+ )
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ if args.randomly_generate_goal:
+ T_w_goal = getRandomlyGeneratedGoal(args)
+ if args.visualizer:
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+ else:
+ T_w_goal = defineGoalPointCLI(robot)
+ T_w_goal.rotation = np.eye(3)
+ T_absgoal_lgoal = pin.SE3.Identity()
+ T_absgoal_lgoal.translation[1] = 0.1
+ T_absgoal_rgoal = pin.SE3.Identity()
+ T_absgoal_rgoal.translation[1] = -0.1
+ moveLDualArm(args, robot, T_w_goal, T_absgoal_lgoal, T_absgoal_rgoal)
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
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 945c100de8c9ed96dc960a2f9b6290669d20e892..f9b373a0908d2cf85d3060502462d6288757a1c7 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
@@ -1,5 +1,7 @@
+from IPython import embed
from smc.control.control_loop_manager import ControlLoopManager
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
from smc.robots.interfaces.force_torque_sensor_interface import (
ForceTorqueOnSingleArmWrist,
)
@@ -8,9 +10,19 @@ from functools import partial
import pinocchio as pin
import numpy as np
from argparse import Namespace
+from collections import deque
+from typing import Callable
-def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _, __):
+def controlLoopClik(
+ args: Namespace,
+ robot: SingleArmInterface,
+ # J err_vec v_cmd
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ T_w_goal: pin.SE3,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
controlLoopClik
---------------
@@ -26,7 +38,7 @@ def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _,
# first check whether we're at the goal
SEerror = T_w_e.actInv(T_w_goal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.args.goal_error:
+ if np.linalg.norm(err_vector) < args.goal_error:
breakFlag = True
J = robot.getJacobian()
# compute the joint velocities based on controller you passed
@@ -47,7 +59,7 @@ def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _,
return breakFlag, save_past_item, log_item
-def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal):
+def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal: pin.SE3) -> None:
"""
moveL
-----
@@ -75,7 +87,9 @@ def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal):
# TODO: implement
-def moveLFollowingLine(args: Namespace, robot, goal_point):
+def moveLFollowingLine(
+ args: Namespace, robot, goal_point: pin.SE3
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
moveLFollowingLine
------------------
@@ -87,17 +101,18 @@ def moveLFollowingLine(args: Namespace, robot, goal_point):
then another version goes in a line and linearly updates the orientation
as it goes
"""
- pass
+ ...
def moveUntilContactControlLoop(
args: Namespace,
robot: ForceTorqueOnSingleArmWrist,
speed: np.ndarray,
- ik_solver,
- i,
- past_data,
-):
+ # J err_vec v_cmd
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
moveUntilContactControlLoop
---------------
@@ -158,26 +173,32 @@ def moveUntilContact(
print("Collision detected!!")
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def controlLoopClikDualArmsOnly(
- robot: AbstractRobotManager, ik_solver, goal_transform, i, past_data
-):
-
- controlLoopClikDualArmsOnly
+def controlLoopClikDualArm(
+ args: Namespace,
+ robot: DualArmInterface,
+ # J err_vec v_cmd
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ T_w_absgoal: pin.SE3,
+ T_abs_lgoal: pin.SE3,
+ T_abs_rgoal: pin.SE3,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ controlLoopClikDualArm
---------------
+ do point to point motion for each arm and its goal.
+ that goal is generated from a single goal that you pass,
+ and an SE3 transformation on the goal for each arm
+ """
breakFlag = False
log_item = {}
- q = robot.q
- T_w_e_left, T_w_e_right = robot.getT_w_e()
- #
- Mgoal_left = robot.Mgoal.act(goal_transform)
- Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+ T_w_lgoal = T_abs_lgoal.act(T_w_absgoal)
+ T_w_rgoal = T_abs_rgoal.act(T_w_absgoal)
- SEerror_left = T_w_e_left.actInv(Mgoal_left)
- SEerror_right = T_w_e_right.actInv(Mgoal_right)
+ SEerror_left = robot.T_w_l.actInv(T_w_lgoal)
+ SEerror_right = robot.T_w_r.actInv(T_w_rgoal)
err_vector_left = pin.log6(SEerror_left).vector
err_vector_right = pin.log6(SEerror_right).vector
@@ -186,174 +207,54 @@ def controlLoopClikDualArmsOnly(
np.linalg.norm(err_vector_right) < robot.args.goal_error
):
breakFlag = True
- J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
- J_left = J_left[:, 3:]
- J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
- J_right = J_right[:, 3:]
-
+ err_vector = np.concatenate((err_vector_left, err_vector_right))
+ J = robot.getJacobian()
# compute the joint velocities based on controller you passed
- qd_left = ik_solver(J_left, err_vector_left)
- qd_right = ik_solver(J_right, err_vector_right)
- # lb, ub = (-0.05 * robot.model.robot.model.velocityLimit, 0.05 * robot.model.robot.model.velocityLimit)
- # qd_left = invKinmQP(J_left, err_vector_left, lb=lb[3:], ub=ub[3:])
- # qd_right = invKinmQP(J_right, err_vector_right, lb=lb[3:], ub=ub[3:])
- qd = qd_left + qd_right
- qd = np.hstack((np.zeros(3), qd))
- robot.sendVelocityCommand(qd)
+ # this works exactly the way you think it does:
+ # the velocities for the other arm are 0
+ # what happens to the base hasn't been investigated but it seems ok
+ v_cmd = ik_solver(J, err_vector)
+ robot.sendVelocityCommand(v_cmd)
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
- # hence the empty dict
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
+ log_item["dqs_cmd"] = v_cmd
return breakFlag, {}, log_item
-"""
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def moveLDualArmsOnly(
- args, robot: AbstractRobotManager, goal_point, goal_transform, run=True
-):
- ===
- moveL
- -----
- does moveL.
- send a SE3 object as goal point.
- if you don't care about rotation, make it np.zeros((3,3))
- ===
- # assert type(goal_point) == pin.pinocchio_pywrap.SE3
+def moveLDualArm(
+ args: Namespace,
+ robot: DualArmInterface,
+ T_w_goal: pin.SE3,
+ T_abs_l: pin.SE3,
+ T_abs_r: pin.SE3,
+ run=True,
+) -> None:
+ """
+ moveLDualArm
+ -----------
+ """
+ # TODO: clean this up
+ robot._mode = DualArmInterface.control_mode.both
ik_solver = getIKSolver(args, robot)
- controlLoop = partial(controlLoopClikDualArmsOnly, robot, ik_solver, goal_transform)
+ controlLoop = partial(
+ controlLoopClikDualArm, args, robot, ik_solver, T_w_goal, T_abs_l, T_abs_r
+ )
# we're not using any past data or logging, hence the empty arguments
log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
- "dqs_cmd": np.zeros(robot.model.nv),
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
}
- loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
if run:
loop_manager.run()
else:
return loop_manager
-"""
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def controlLoopClikDualArm(
- robot: DualArmInterface, ik_solver, goal_transform, i, past_data
-):
-# controlLoopClikDualArm
-# ---------------
-# do point to point motion for each arm and its goal.
-# that goal is generated from a single goal that you pass,
-# and a transformation on the goal you also pass.
-# the transformation is done in goal's frame (goal is and has
-# to be an SE3 object).
-# the transform is designed for the left arm,
-# and its inverse is applied for the right arm.
-
-
-
-
- breakFlag = False
- log_item = {}
- q = robot.q
- T_w_e_left, T_w_e_right = robot.getT_w_e()
- #
- Mgoal_left = goal_transform.act(robot.Mgoal)
- Mgoal_right = goal_transform.inverse().act(robot.Mgoal)
- # print("robot.Mgoal", robot.Mgoal)
- # print("Mgoal_left", Mgoal_left)
- # print("Mgoal_right", Mgoal_right)
-
- SEerror_left = T_w_e_left.actInv(Mgoal_left)
- SEerror_right = T_w_e_right.actInv(Mgoal_right)
-
- err_vector_left = pin.log6(SEerror_left).vector
- err_vector_right = pin.log6(SEerror_right).vector
-
- if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (
- np.linalg.norm(err_vector_right) < robot.args.goal_error
- ):
- breakFlag = True
- J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
- J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
- # compute the joint velocities based on controller you passed
- # this works exactly the way you think it does:
- # the velocities for the other arm are 0
- # what happens to the base hasn't been investigated but it seems ok
- qd_left = ik_solver(J_left, err_vector_left)
- qd_right = ik_solver(J_right, err_vector_right)
- qd = qd_left + qd_right
- robot.sendVelocityCommand(qd)
-
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
- # hence the empty dict
- return breakFlag, {}, log_item
-"""
-
-
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def clikDualArm(args, robot, goal, goal_transform, run=True):
-#
-# clikDualArm
-# -----------
-#
- robot.Mgoal = copy.deepcopy(goal)
- ik_solver = getClikController(args, robot)
- controlLoop = partial(controlLoopClikDualArm, robot, ik_solver, goal_transform)
- # we're not using any past data or logging, hence the empty arguments
- log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv),
- 'dqs_cmd' : np.zeros(robot.model.nv),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- if run:
- loop_manager.run()
- else:
- return loop_manager
-"""
-
-
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def controlLoopClikArmOnly(robot, ik_solver, _, _):
- breakFlag = False
- log_item = {}
- q = robot.q
- T_w_e = robot.getT_w_e()
- # first check whether we're at the goal
- SEerror = T_w_e.actInv(robot.Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.args.goal_error:
- breakFlag = True
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
- # cut off base from jacobian
- J = J[:, 3:]
- # compute the joint velocities based on controller you passed
- qd = ik_solver(J, err_vector)
- # add the missing velocities back
- qd = np.hstack((np.zeros(3), qd))
- robot.sendVelocityCommand(qd)
-
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
- # hence the empty dict
- return breakFlag, {}, log_item
-"""
# NOTE: this shit makes no sense, it's old and broken
# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index f517d9b30a99bbc24356c76bf3986841b4d3a794..01f72443a97a349cce51ffe725d2073f921bc424 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -1,10 +1,18 @@
+from typing import Callable
import numpy as np
from qpsolvers import solve_qp
import proxsuite
+
from functools import partial
+from argparse import Namespace
+
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
-def getIKSolver(args, robot):
+def getIKSolver(
+ args: Namespace, robot: SingleArmInterface | DualArmInterface
+) -> Callable[[np.ndarray, np.ndarray], np.ndarray]:
"""
getIKSolver
-----------------
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index 6b92aabf90dfc76c978df72a0f3a1646af65216b..9951ff1ddc1f0ee6c1d80cc3fdc2ea6214ab0da8 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -149,17 +149,11 @@ class ControlLoopManager:
"q": self.robot_manager._q,
}
)
- if self.robot_manager.robot_name != "yumi":
- self.robot_manager.visualizer_manager.sendCommand(
- {
- "T_w_e": self.robot_manager.T_w_e,
- }
- )
- else:
- T_w_e_left, T_w_e_right = self.robot_manager.T_w_e
- self.robot_manager.visualizer_manager.sendCommand(
- {"T_w_e": T_w_e_left}
- )
+ self.robot_manager.visualizer_manager.sendCommand(
+ {
+ "T_w_e": self.robot_manager.T_w_e,
+ }
+ )
if self.robot_manager.robot_name == "heron":
T_base = self.robot_manager.data.oMi[1]
self.robot_manager.visualizer_manager.sendCommand(
@@ -170,7 +164,7 @@ class ControlLoopManager:
# self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
if self.args.visualize_collision_approximation:
- raise NotADirectoryError
+ raise NotImplementedError
# TODO: here call robot manager's update ellipses function
if self.args.plotter:
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
index 3f8ce0e7af11f2e875f34c49c0297c49208b6495..0e0aeb6cc2d4f441792bae2ef601b53a412bb6a3 100644
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -1,3 +1,4 @@
+from argparse import Namespace
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.control.control_loop_manager import ControlLoopManager
from smc.control.optimal_control.point_to_point_croco_ocp import (
@@ -10,7 +11,14 @@ import numpy as np
from functools import partial
-def CrocoEEP2PMPCControlLoop(args, robot: SingleArmInterface, ocp, T_w_goal, _, __):
+def CrocoEEP2PMPCControlLoop(
+ args: Namespace,
+ robot: SingleArmInterface,
+ ocp,
+ T_w_goal: pin.SE3,
+ _: int,
+ __: dict[str, deque[np.ndarray]],
+):
"""
CrocoIKMPCControlLoop
---------------------
@@ -42,7 +50,7 @@ def CrocoEEP2PMPCControlLoop(args, robot: SingleArmInterface, ocp, T_w_goal, _,
return breakFlag, save_past_dict, log_item
-def EEP2PMPC(args, robot, T_w_goal, run=True):
+def EEP2PMPC(args: Namespace, robot, T_w_goal: pin.SE3, run=True):
"""
IKMPC
-----
@@ -79,8 +87,8 @@ def CrocoP2PEEAndBaseMPCControlLoop(
ocp,
T_w_eegoal: pin.SE3,
p_basegoal: np.ndarray,
- _,
- __,
+ _: int,
+ __: dict[str, deque[np.ndarray]],
):
"""
CrocoP2PEEAndBaseMPCControlLoop
diff --git a/python/smc/robots/implementations/__init__.py b/python/smc/robots/implementations/__init__.py
index 6a9cc722e3707207c452e8f44e8ef2e990e2e8be..98afce5bf00207228bb2e84c2c07912553ee4f22 100644
--- a/python/smc/robots/implementations/__init__.py
+++ b/python/smc/robots/implementations/__init__.py
@@ -1,6 +1,7 @@
# from .heron import RobotManagerHeron, SimulatedHeronRobotManager
from .heron import *
from .mir import *
+from .yumi import *
from .mobile_yumi import *
from .ur5e import RobotManagerUR5e, SimulatedUR5eRobotManager
from importlib.util import find_spec
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index b4a9edb936082608516171a8433dcc84d75aa94f..45ef20f09b975858509c464090b39978cb6b388f 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -9,6 +9,7 @@ from smc.robots.interfaces.mobile_base_interface import (
from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
from smc.robots.implementations.ur5e import get_model
+from argparse import Namespace
import time
import numpy as np
import pinocchio as pin
@@ -50,7 +51,7 @@ class RobotManagerHeron(
class SimulatedHeronRobotManager(AbstractSimulatedRobotManager, RobotManagerHeron):
- def __init__(self, args):
+ def __init__(self, args: Namespace):
if args.debug_prints:
print("SimulatedRobotManagerHeron init")
super().__init__(args)
@@ -58,7 +59,7 @@ class SimulatedHeronRobotManager(AbstractSimulatedRobotManager, RobotManagerHero
# NOTE: overriding wrench stuff here
# there can be a debated whether there should be a simulated forcetorquesensorinterface,
# but it's annoying as hell and there is no immediate benefit in solving this problem
- def _updateWrench(self):
+ def _updateWrench(self) -> None:
self._wrench_base = np.random.random(6)
# NOTE: create a robot_math module, make this mapping a function called
# mapse3ToDifferent frame or something like that
diff --git a/python/smc/robots/implementations/yumi.py b/python/smc/robots/implementations/yumi.py
index b96ac6939988c210ab3b253e677a35d8dc7d9c42..be195f2b15091b85e151ab399878f0188716a30b 100644
--- a/python/smc/robots/implementations/yumi.py
+++ b/python/smc/robots/implementations/yumi.py
@@ -1,11 +1,13 @@
+from smc.robots.abstract_simulated_robot import AbstractSimulatedRobotManager
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from argparse import Namespace
from importlib.resources import files
from os import path
import pinocchio as pin
-class RobotManagerYuMi(DualArmInterface):
+class AbstractYuMiRobotManager(DualArmInterface):
@property
def model(self):
@@ -25,11 +27,25 @@ class RobotManagerYuMi(DualArmInterface):
def __init__(self, args):
if args.debug_prints:
- print("RobotManagerUR5e init")
- self._model, self._collision_model, self._visual_model, self._data = get_model()
- self._ee_frame_id = self.model.getFrameId("tool0")
+ print("RobotManagerYuMi init")
+ self._model, self._collision_model, self._visual_model, self._data = (
+ get_yumi_model()
+ )
+ self._l_ee_frame_id = self.model.getFrameId("robl_tool0")
+ self._r_ee_frame_id = self.model.getFrameId("robr_tool0")
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
+
+ self._mode: DualArmInterface.control_mode = DualArmInterface.control_mode.both
+ super().__init__(args)
+
+
+class SimulatedYuMiRobotManager(
+ AbstractSimulatedRobotManager, AbstractYuMiRobotManager
+):
+ def __init__(self, args: Namespace):
+ if args.debug_prints:
+ print("SimulatedYuMiRobotManager init")
super().__init__(args)
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index f93465e25015fd6390e72073570ee29c656aeb24..468ec8d339c5c7225d57038d5d4153d02f89a99c 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -4,6 +4,7 @@ import abc
import numpy as np
import pinocchio as pin
from argparse import Namespace
+from enum import Enum
class DualArmInterface(SingleArmInterface):
@@ -15,6 +16,8 @@ class DualArmInterface(SingleArmInterface):
- r stands for right
"""
+ control_mode = Enum("mode", [("both", 3), ("left", 4), ("right", 5)])
+
# NOTE: you need to fill in the specific names from the urdf here for your robot
# (better than putting in a magic number)
# self._l_ee_frame_id = self.model.getFrameId("left_ee_name")
@@ -31,6 +34,58 @@ class DualArmInterface(SingleArmInterface):
print("DualArmInterface init")
super().__init__(args)
+ @property
+ def mode(self) -> control_mode:
+ return self._mode
+
+ @mode.setter
+ def setMode(self, mode: control_mode) -> None:
+ assert type(mode) is self.control_mode
+ self._mode = mode
+
+ @property
+ def q(self) -> np.ndarray:
+ # NOTE: left should be on the left side of the joint values array
+ if self._mode.value == 4:
+ return self._q[: self.model.nq // 2]
+
+ # NOTE: right should be on the left side of the joint values array
+ if self._mode.value == 5:
+ return self._q[self.model.nq // 2 :]
+
+ return self._q.copy()
+
+ @property
+ def nq(self):
+ if self._mode.value == 4:
+ return self.model.nq // 2
+
+ if self._mode.value == 5:
+ return self.model.nq // 2
+
+ return self.model.nq
+
+ @property
+ def v(self) -> np.ndarray:
+ if self._mode.value == 4:
+ return self._v[: self.model.nv // 2]
+
+ if self._mode.value == 5:
+ return self._v[: self.model.nv // 2]
+
+ return self._v.copy()
+
+ @property
+ def nv(self):
+ if self._mode.value == 4:
+ return self.model.nv // 2
+
+ if self._mode.value == 5:
+ return self.model.nv // 2
+
+ return self.model.nv
+
+ # NOTE: might be misleading and unhelpful
@property
def T_w_e(self):
return self.T_w_abs
@@ -54,11 +109,12 @@ class DualArmInterface(SingleArmInterface):
# T_abs_l and T_abs_r are relative transformations between the absolute dual-arm frame,
# and the left and right end-effector frames respectively
@property
- @abc.abstractmethod
- def T_abs_r(self) -> pin.SE3: ...
+ def T_abs_r(self) -> pin.SE3:
+ return self._T_w_abs.actInv(self._T_w_r)
+
@property
- @abc.abstractmethod
- def T_abs_l(self) -> pin.SE3: ...
+ def T_abs_l(self) -> pin.SE3:
+ return self._T_w_abs.actInv(self._T_w_l)
@property
def T_w_abs(self) -> pin.SE3:
@@ -133,12 +189,16 @@ class DualArmInterface(SingleArmInterface):
# the other arm filled with zeros
J_left = pin.computeFrameJacobian(
self.model, self.data, self._q, self._l_ee_frame_id
- )
+ )[:, : self.model.nq // 2]
+ if self._mode.value == 4:
+ return J_left
J_right = pin.computeFrameJacobian(
self.model, self.data, self._q, self._r_ee_frame_id
- )
+ )[:, self.model.nq // 2 :]
+ if self._mode.value == 5:
+ return J_right
J = np.zeros((12, self.nq))
- J[: self.nq, :6] = J_left
- J[self.nq :, 6:] = J_right
+ J[:6, : self.model.nq // 2] = J_left
+ J[6:, self.model.nq // 2 :] = J_right
return J
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index 6f83f346604f5bd9b32d13f17863195d2e758544..f55a5617a98d5e9b4d4e1e58b3507f616d96ece5 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -19,15 +19,15 @@ def getMinimalArgParser():
Simple Manipulator Control",
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
)
- #################################################
- # general arguments: connection, plotting etc #
- #################################################
+ ########################################################
+ # general arguments: robot, simulation, plotting etc #
+ ########################################################
parser.add_argument(
"--robot",
type=str,
help="which robot you're running or simulating",
default="ur5e",
- choices=["ur5e", "heron", "heronros", "gripperlessur5e", "mirros", "yumi"],
+ choices=["ur5e", "heron", "yumi", "mobile_yumi"],
)
parser.add_argument(
"--real",
@@ -209,7 +209,7 @@ def defineGoalPointCLI(robot):
print(
"base frame end-effector pose from pinocchio:\n",
*robot.data.oMi[6].translation.round(4),
- *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4)
+ *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4),
)
print("UR5e TCP:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
# remain with the current orientation
@@ -260,6 +260,16 @@ def getRobotFromArgs(args: argparse.Namespace) -> AbstractRobotManager:
# return RealHeronRobotManager(args)
else:
return SimulatedHeronRobotManager(args)
+ if args.robot == "yumi":
+ if args.real:
+ pass
+ # TODO: finish it
+ # return RealYuMiRobotManager(args)
+ else:
+ return SimulatedYuMiRobotManager(args)
+ raise NotImplementedError(
+ f"robot {args.robot} is not supported! run the script you ran with --help to see what's available"
+ )
# if args.robot == "mir":