diff --git a/examples/cart_pulling/todos.toml b/examples/cart_pulling/todos.toml
index 685bd60eda1049799be7e282450da2ebd6721828..e07e154764643a42d3e9c45f750d40d8273936e5 100644
--- a/examples/cart_pulling/todos.toml
+++ b/examples/cart_pulling/todos.toml
@@ -552,12 +552,12 @@ 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
+1. p2p dual arm only mpc - DONE
+2. p2p dual arm and base - DONE
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
+6. path following from planner dual arm and base - DONE
"""
priority = 1
is_done = false
@@ -575,7 +575,7 @@ description = """
refactor end-effector path following
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["dual arm feature", "whole-body feature", "dual arm p2p and path following"]
purpose = """
@@ -647,6 +647,39 @@ purpose = """
hours_required = 0
assignee = "Marko"
+[[project_plan.action_items.action_items.action_items]]
+name = "compile pin+croco stack to allow parallel execution"
+description = """
+try seeing if there's some parameter somewhere that could be set to parallelise.
+
+a trillion percent it's runnable on multiple cores and it's just not happening now for some reason.
+"""
+priority = 1
+is_done = false
+deadline = 2025-02-14
+dependencies = []
+purpose = """
+solving faster means longer time horizon and higher control frequency, both of which increase performance substantially
+"""
+hours_required = 3
+assignee = "Marko"
+
+[[project_plan.action_items.action_items.action_items]]
+name = "tune controller parameters"
+description = """
+assign different scalars to different costs, see what that does.
+would also be interesting do change them depending on for example the ratios between different errors - the cost with the highest error is the highest, thereby forcing a quick recovery idk. another idea is changing the ratio between tracking cost and regularization depending on the tracking error norm - the closer you are the lower the regularization and higher the tracking cost.
+"""
+priority = 1
+is_done = false
+deadline = 2025-02-14
+dependencies = []
+purpose = """
+getting much better performance just by turning knobs (also tuning is pretty much mandatory)
+"""
+hours_required = 3
+assignee = "Marko"
+
[[project_plan.action_items.action_items.action_items]]
name = "rewrite ee construction from past path to be more efficient"
description = """
diff --git a/examples/drawing/drawing_from_input_drawing.py b/examples/drawing/drawing_from_input_drawing.py
index 9f9859e474e5f018316d4fad95c20aea0102a7ac..4f9a66a38cc0a7b297dd21b418769706f10a1d3f 100644
--- a/examples/drawing/drawing_from_input_drawing.py
+++ b/examples/drawing/drawing_from_input_drawing.py
@@ -1,180 +1,52 @@
-# PYTHON_ARGCOMPLETE_OK
-# TODO: make 1/beta = 5
-import pinocchio as pin
-import numpy as np
-import matplotlib
-import argparse, argcomplete
-import time
-import pickle
-from functools import partial
-from smc.visualize.visualize import plotFromDict
+from utils import getArgs
+from get_marker import getMarker
+from find_marker_offset import findMarkerOffset
+
+from smc.robots.interfaces.force_torque_sensor_interface import (
+ ForceTorqueOnSingleArmWrist,
+)
from smc.util.draw_path import drawPath
-from smc.dmp.dmp import (
- getDMPArgs,
+from smc.control.dmp.dmp import (
DMP,
NoTC,
TCVelAccConstrained,
- followDMP,
)
-from smc.clik.clik import (
- getClikArgs,
- getClikController,
+from smc.control.cartesian_space.ik_solvers import getIKSolver
+from smc.control.cartesian_space import (
moveL,
- moveUntilContact,
- controlLoopClik,
compliantMoveL,
+)
+from smc.path_generation.cartesian_to_joint_space_path_mapper import (
clikCartesianPathIntoJointPath,
)
from smc.util.map2DPathTo3DPlane import map2DPathTo3DPlane
-from smc.managers import (
- getMinimalArgParser,
- ControlLoopManager,
- RobotManager,
-)
+from smc import getRobotFromArgs
+from smc.control.control_loop_manager import ControlLoopManager
from smc.util.calib_board_hacks import (
- getBoardCalibrationArgs,
calibratePlane,
)
-from smc.basics.basics import moveJPI
+
+import pinocchio as pin
+import numpy as np
+import matplotlib
+from argparse import Namespace
+import pickle
+from functools import partial
+from collections import deque
#######################################################################
# arguments #
#######################################################################
-def getArgs():
- parser = getMinimalArgParser()
- parser = getClikArgs(parser)
- parser = getDMPArgs(parser)
- parser = getBoardCalibrationArgs(parser)
- parser.description = "Make a drawing on screen,\
- watch the robot do it on the whiteboard."
- parser.add_argument(
- "--kp",
- type=float,
- help="proportial control constant for position errors",
- default=1.0,
- )
- parser.add_argument(
- "--mm-into-board",
- type=float,
- help="number of milimiters the path is into the board",
- default=3.0,
- )
- parser.add_argument(
- "--kv", type=float, help="damping in impedance control", default=0.001
- )
- parser.add_argument(
- "--draw-new",
- action=argparse.BooleanOptionalAction,
- help="whether draw a new picture, or use the saved path path_in_pixels.csv",
- default=True,
- )
- parser.add_argument(
- "--pick-up-marker",
- action=argparse.BooleanOptionalAction,
- help="""
- whether the robot should pick up the marker.
- NOTE: THIS IS FROM A PREDEFINED LOCATION.
- """,
- default=False,
- )
- parser.add_argument(
- "--find-marker-offset",
- action=argparse.BooleanOptionalAction,
- help="""
- whether you want to do find marker offset (recalculate TCP
- based on the marker""",
- default=True,
- )
- parser.add_argument(
- "--board-wiping",
- action=argparse.BooleanOptionalAction,
- help="are you wiping the board (default is no because you're writing)",
- default=False,
- )
- argcomplete.autocomplete(parser)
- args = parser.parse_args()
- return args
-
-
-def getMarker(args, robot, plane_pose):
- """
- getMarker
- ---------
- get marker from user in a blind handover
- RUN THIS IN SIM FIRST TO SEE IF IT MAKES SENSE.
- if not, generate a different joint trajectory for your situation.
- """
- # load traj
- file = open("./data/from_writing_to_handover.pickle_save", "rb")
- point_dict = pickle.load(file)
- file.close()
- #####################
- # go toward user #
- #####################
- # this is more than enough, and it will be the same for both
- tau0 = 5
- # setting up array for dmp
- # TODO: make the dmp general already
- for i in range(len(point_dict["qs"])):
- point_dict["qs"][i] = point_dict["qs"][i][:6]
- qs = np.array(point_dict["qs"])
-
- followDMP(args, robot, qs, tau0)
- robot.sendQd(np.zeros(robot.model.nq))
-
- ##########################################
- # blind handover (open/close on timer) #
- ##########################################
- robot.openGripper()
- time.sleep(5)
- robot.closeGripper()
- time.sleep(3)
-
- #############
- # go back #
- #############
- point_dict["qs"].reverse()
- # setting up array for dmp
- qs = np.array(point_dict["qs"])
- followDMP(args, robot, qs, tau0)
-
-
-def findMarkerOffset(args, robot: RobotManager, plane_pose: pin.SE3):
- """
- findMarkerOffset
- ---------------
- This relies on having the correct orientation of the plane
- and the correct translation vector for top-left corner.
- Idea is you pick up the marker, go to the top corner,
- touch it, and see the difference between that and the translation vector.
- Obviously it's just a hacked solution, but it works so who cares.
- """
- above_starting_write_point = pin.SE3.Identity()
- above_starting_write_point.translation[2] = -0.2
- above_starting_write_point = plane_pose.act(above_starting_write_point)
- print("going to above plane pose point", above_starting_write_point)
- compliantMoveL(args, robot, above_starting_write_point)
-
- # this is in the end-effector frame, so this means going straight down
- # because we are using the body jacobians in our clik
- speed = np.zeros(6)
- speed[2] = 0.02
- moveUntilContact(args, robot, speed)
- # we use the pin coordinate system because that's what's
- # the correct thing long term accross different robots etc
- current_translation = robot.getT_w_e().translation
- # i only care about the z because i'm fixing the path atm
- # but, let's account for the possible milimiter offset 'cos why not
- marker_offset = np.linalg.norm(plane_pose.translation - current_translation)
-
- print("going back")
- compliantMoveL(args, robot, above_starting_write_point)
- return marker_offset
-
-
-def controlLoopWriting(args, robot: RobotManager, dmp, tc, i, past_data):
+def controlLoopWriting(
+ args: Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+ dmp: DMP,
+ tc,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
controlLoopWriting
-----------------------
@@ -187,11 +59,10 @@ def controlLoopWriting(args, robot: RobotManager, dmp, tc, i, past_data):
# temporal coupling step
tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
dmp.set_tau(tau_dmp)
- q = robot.getQ()
- T_w_e = robot.getT_w_e()
+ q = robot.q
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.0]))
- wrench = robot.getWrench()
+ wrench = robot.wrench
save_past_dict["wrench"] = wrench.copy()
# rolling average
# wrench = np.average(np.array(past_data['wrench']), axis=0)
@@ -204,17 +75,16 @@ def controlLoopWriting(args, robot: RobotManager, dmp, tc, i, past_data):
)
wrench = Z @ wrench
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
- dq = robot.getQd()[:6].reshape((6, 1))
+ J = robot.getJacobian()
# get joint
tau = J.T @ wrench
- tau = tau[:6].reshape((6, 1))
+ tau = tau.reshape((-1, 1))
# compute control law:
# - feedforward the velocity and the force reading
# - feedback the position
# TODO solve this q[:6] bs (clean it up)
- vel_cmd = dmp.vel + args.kp * (dmp.pos - q[:6].reshape((6, 1))) + args.alpha * tau
- robot.sendQd(vel_cmd)
+ vel_cmd = dmp.vel + args.kp * (dmp.pos - q.reshape((-1, 1))) + args.alpha * tau
+ robot.sendVelocityCommand(vel_cmd)
# tau0 is the minimum time needed for dmp
# 500 is the frequency
@@ -228,9 +98,9 @@ def controlLoopWriting(args, robot: RobotManager, dmp, tc, i, past_data):
# log what you said you'd log
# TODO fix the q6 situation (hide this)
- log_item["qs"] = q[:6].reshape((6,))
+ log_item["qs"] = robot.q
log_item["dmp_qs"] = dmp.pos.reshape((6,))
- log_item["dqs"] = dq.reshape((6,))
+ log_item["dqs"] = robot.v
log_item["dmp_dqs"] = dmp.vel.reshape((6,))
log_item["wrench"] = wrench.reshape((6,))
log_item["tau"] = tau.reshape((6,))
@@ -238,14 +108,18 @@ def controlLoopWriting(args, robot: RobotManager, dmp, tc, i, past_data):
return breakFlag, save_past_dict, log_item
-def write(args, robot: RobotManager, joint_trajectory):
+def write(
+ joint_trajectory: np.ndarray,
+ args: Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+) -> None:
# create DMP based on the trajectory
dmp = DMP(joint_trajectory, a_s=1.0)
if not args.temporal_coupling:
tc = NoTC()
else:
- v_max_ndarray = np.ones(robot.n_arm_joints) * robot.max_qd
- a_max_ndarray = np.ones(robot.n_arm_joints) * args.acceleration
+ v_max_ndarray = np.ones(robot.nq) * robot._max_v
+ a_max_ndarray = np.ones(robot.nq) * args.acceleration
tc = TCVelAccConstrained(
args.gamma_nominal, args.gamma_a, v_max_ndarray, a_max_ndarray, args.eps_tc
)
@@ -253,32 +127,28 @@ def write(args, robot: RobotManager, joint_trajectory):
print("going to starting write position")
dmp.step(1 / 500)
first_q = dmp.pos.reshape((6,))
- first_q = list(first_q)
- first_q.append(0.0)
- first_q.append(0.0)
- first_q = np.array(first_q)
# move to initial pose
- mtool = robot.getT_w_e(q_given=first_q)
+ T_w_goal = robot.computeT_w_e(first_q)
# start a bit above
go_away_from_plane_transf = pin.SE3.Identity()
go_away_from_plane_transf.translation[2] = -1 * args.mm_into_board
- mtool = mtool.act(go_away_from_plane_transf)
+ T_w_goal = T_w_goal.act(go_away_from_plane_transf)
if not args.board_wiping:
- compliantMoveL(args, robot, mtool)
+ compliantMoveL(T_w_goal, args, robot)
else:
- moveL(args, robot, mtool)
+ moveL(args, robot, T_w_goal)
save_past_dict = {
"wrench": np.zeros(6),
}
# here you give it it's initial value
log_item = {
- "qs": np.zeros(robot.n_arm_joints),
- "dmp_qs": np.zeros(robot.n_arm_joints),
- "dqs": np.zeros(robot.n_arm_joints),
- "dmp_dqs": np.zeros(robot.n_arm_joints),
+ "qs": np.zeros(robot.nq),
+ "dmp_qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "dmp_dqs": np.zeros(robot.nq),
"wrench": np.zeros(6),
- "tau": np.zeros(robot.n_arm_joints),
+ "tau": np.zeros(robot.nv),
}
# moveJ(args, robot, dmp.pos.reshape((6,)))
controlLoop = partial(controlLoopWriting, args, robot, dmp, tc)
@@ -289,11 +159,11 @@ def write(args, robot: RobotManager, joint_trajectory):
loop_manager.run()
print("move a bit back")
- T_w_e = robot.getT_w_e()
+ T_w_e = robot.T_w_e
go_away_from_plane_transf = pin.SE3.Identity()
go_away_from_plane_transf.translation[2] = -0.1
goal = T_w_e.act(go_away_from_plane_transf)
- compliantMoveL(args, robot, goal)
+ compliantMoveL(goal, args, robot)
if __name__ == "__main__":
@@ -306,11 +176,14 @@ if __name__ == "__main__":
assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
args.mm_into_board = args.mm_into_board / 1000
print(args)
- robot = RobotManager(args)
- if args.pinocchio_only:
- rand_pertb = np.zeros(8)
- rand_pertb[:6] = np.random.random(6) * 0.1
- robot.q = (
+ robot = getRobotFromArgs(args)
+ # NOTE: could be force sensing in joints as well, but that's not implemented yet
+ assert issubclass(robot.__class__, ForceTorqueOnSingleArmWrist)
+ if not args.real:
+ rand_pertb = np.random.random(robot.nq) * 0.1
+ if args.robot != "ur5e":
+ raise NotImplementedError
+ robot._q = (
np.array([1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0]) + rand_pertb
)
robot._step()
@@ -354,15 +227,15 @@ if __name__ == "__main__":
path_points_3D = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
if args.pick_up_marker:
# raise NotImplementedError("sorry")
- getMarker(args, robot, None)
+ getMarker(args, robot)
# marker moves between runs, i change markers etc,
# this goes to start, goes down to touch the board - that's the marker
# length aka offset (also handles incorrect frames)
if args.find_marker_offset:
# find the marker offset
- marker_offset = findMarkerOffset(args, robot, plane_pose)
- robot.sendQd(np.zeros(6))
+ marker_offset = findMarkerOffset(plane_pose, args, robot)
+ robot.sendVelocityCommand(np.zeros(robot.nv))
print("marker_offset", marker_offset)
# we're going in a bit deeper
path_points_3D = path_points_3D + np.array(
@@ -391,7 +264,7 @@ if __name__ == "__main__":
it has to look reasonable, otherwise we can't run it!
"""
)
- clikController = getClikController(args, robot)
+ clikController = getIKSolver(args, robot)
joint_trajectory = clikCartesianPathIntoJointPath(
args, robot, path, clikController, q_init, plane_pose
)
@@ -409,16 +282,14 @@ if __name__ == "__main__":
joint_trajectory = np.genfromtxt(joint_trajectory_file_path, delimiter=",")
if answer:
- write(args, robot, joint_trajectory)
+ write(joint_trajectory, args, robot)
- if not args.pinocchio_only:
+ if args.real:
robot.stopRobot()
if args.save_log:
- robot.log_manager.saveLog()
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
if args.visualizer:
robot.killManipulatorVisualizer()
-
- if args.save_log:
- robot.log_manager.plotAllControlLoops()
diff --git a/examples/drawing/find_marker_offset.py b/examples/drawing/find_marker_offset.py
new file mode 100644
index 0000000000000000000000000000000000000000..c1e9db16c32e5ccd6618e36a530dd3f291eb07a0
--- /dev/null
+++ b/examples/drawing/find_marker_offset.py
@@ -0,0 +1,46 @@
+from smc.robots.interfaces.force_torque_sensor_interface import (
+ ForceTorqueOnSingleArmWrist,
+)
+from smc.control.cartesian_space import (
+ moveUntilContact,
+ compliantMoveL,
+)
+
+import pinocchio as pin
+import numpy as np
+from argparse import Namespace
+
+
+def findMarkerOffset(
+ plane_pose: pin.SE3, args: Namespace, robot: ForceTorqueOnSingleArmWrist
+) -> float:
+ """
+ findMarkerOffset
+ ---------------
+ This relies on having the correct orientation of the plane
+ and the correct translation vector for top-left corner.
+ Idea is you pick up the marker, go to the top corner,
+ touch it, and see the difference between that and the translation vector.
+ Obviously it's just a hacked solution, but it works so who cares.
+ """
+ above_starting_write_point = pin.SE3.Identity()
+ above_starting_write_point.translation[2] = -0.2
+ above_starting_write_point = plane_pose.act(above_starting_write_point)
+ print("going to above plane pose point", above_starting_write_point)
+ compliantMoveL(above_starting_write_point, args, robot)
+
+ # this is in the end-effector frame, so this means going straight down
+ # because we are using the body jacobians in our clik
+ speed = np.zeros(6)
+ speed[2] = 0.02
+ moveUntilContact(args, robot, speed)
+ # we use the pin coordinate system because that's what's
+ # the correct thing long term accross different robots etc
+ current_translation = robot.T_w_e.translation
+ # i only care about the z because i'm fixing the path atm
+ # but, let's account for the possible milimiter offset 'cos why not
+ marker_offset = np.linalg.norm(plane_pose.translation - current_translation)
+
+ print("going back")
+ compliantMoveL(above_starting_write_point, args, robot)
+ return marker_offset
diff --git a/examples/drawing/get_marker.py b/examples/drawing/get_marker.py
new file mode 100644
index 0000000000000000000000000000000000000000..aa6bd57be7628cb0be3d97249c76cf8b8468b168
--- /dev/null
+++ b/examples/drawing/get_marker.py
@@ -0,0 +1,50 @@
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.dmp import followDMP
+
+from argparse import Namespace
+import pickle
+import numpy as np
+import time
+
+
+def getMarker(args: Namespace, robot: SingleArmInterface) -> None:
+ """
+ getMarker
+ ---------
+ get marker from user in a blind handover
+ RUN THIS IN SIM FIRST TO SEE IF IT MAKES SENSE.
+ if not, generate a different joint trajectory for your situation.
+ """
+ # load traj
+ file = open("./data/from_writing_to_handover.pickle_save", "rb")
+ point_dict = pickle.load(file)
+ file.close()
+ #####################
+ # go toward user #
+ #####################
+ # this is more than enough, and it will be the same for both
+ tau0 = 5
+ # setting up array for dmp
+ # TODO: make the dmp general already
+ for i in range(len(point_dict["qs"])):
+ point_dict["qs"][i] = point_dict["qs"][i][:6]
+ qs = np.array(point_dict["qs"])
+
+ followDMP(args, robot, qs, tau0)
+ robot.sendVelocityCommand(np.zeros(robot.nv))
+
+ ##########################################
+ # blind handover (open/close on timer) #
+ ##########################################
+ robot.openGripper()
+ time.sleep(5)
+ robot.closeGripper()
+ time.sleep(3)
+
+ #############
+ # go back #
+ #############
+ point_dict["qs"].reverse()
+ # setting up array for dmp
+ qs = np.array(point_dict["qs"])
+ followDMP(args, robot, qs, tau0)
diff --git a/examples/drawing/plane_pose.pickle_save b/examples/drawing/parameters/plane_pose.pickle_save
similarity index 100%
rename from examples/drawing/plane_pose.pickle_save
rename to examples/drawing/parameters/plane_pose.pickle_save
diff --git a/examples/drawing/wiping_path.csv_save b/examples/drawing/parameters/wiping_path.csv_save
similarity index 100%
rename from examples/drawing/wiping_path.csv_save
rename to examples/drawing/parameters/wiping_path.csv_save
diff --git a/examples/drawing/utils.py b/examples/drawing/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..407c0f1042a7c33b469370e7ef3dcb23ca3fbe14
--- /dev/null
+++ b/examples/drawing/utils.py
@@ -0,0 +1,60 @@
+from smc import getMinimalArgParser
+from smc.control.cartesian_space import getClikArgs
+from smc.control.dmp.dmp import getDMPArgs
+from smc.util.calib_board_hacks import getBoardCalibrationArgs
+import argparse
+
+
+def getArgs() -> argparse.Namespace:
+ parser = getMinimalArgParser()
+ parser = getClikArgs(parser)
+ parser = getDMPArgs(parser)
+ parser = getBoardCalibrationArgs(parser)
+ parser.description = "Make a drawing on screen,\
+ watch the robot do it on the whiteboard."
+ parser.add_argument(
+ "--kp",
+ type=float,
+ help="proportial control constant for position errors",
+ default=1.0,
+ )
+ parser.add_argument(
+ "--mm-into-board",
+ type=float,
+ help="number of milimiters the path is into the board",
+ default=3.0,
+ )
+ parser.add_argument(
+ "--kv", type=float, help="damping in impedance control", default=0.001
+ )
+ parser.add_argument(
+ "--draw-new",
+ action=argparse.BooleanOptionalAction,
+ help="whether draw a new picture, or use the saved path path_in_pixels.csv",
+ default=True,
+ )
+ parser.add_argument(
+ "--pick-up-marker",
+ action=argparse.BooleanOptionalAction,
+ help="""
+ whether the robot should pick up the marker.
+ NOTE: THIS IS FROM A PREDEFINED LOCATION.
+ """,
+ default=False,
+ )
+ parser.add_argument(
+ "--find-marker-offset",
+ action=argparse.BooleanOptionalAction,
+ help="""
+ whether you want to do find marker offset (recalculate TCP
+ based on the marker""",
+ default=True,
+ )
+ parser.add_argument(
+ "--board-wiping",
+ action=argparse.BooleanOptionalAction,
+ help="are you wiping the board (default is no because you're writing)",
+ default=False,
+ )
+ args = parser.parse_args()
+ return args
diff --git a/python/smc/control/cartesian_space/__init__.py b/python/smc/control/cartesian_space/__init__.py
index 374a25bf46e43fed5be82c41b0a5b0f2a0eadceb..63b5ce730d4585b26d35408774c63fe0587815c5 100644
--- a/python/smc/control/cartesian_space/__init__.py
+++ b/python/smc/control/cartesian_space/__init__.py
@@ -1,6 +1,7 @@
from .cartesian_space_compliant_control import *
from .cartesian_space_point_to_point import *
from .cartesian_space_trajectory_following import *
+from .ik_solvers import *
from .utils import getClikArgs
diff --git a/python/smc/control/dmp/dmp.py b/python/smc/control/dmp/dmp.py
index dbd1028325fdd855e71a1e3042f4691e48df5392..0737daa6ce0f29413531d4efb53471f9b17d3e9a 100644
--- a/python/smc/control/dmp/dmp.py
+++ b/python/smc/control/dmp/dmp.py
@@ -1,8 +1,10 @@
+from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.control.control_loop_manager import ControlLoopManager
+
import numpy as np
import argparse
-from smc.robots.robotmanager_abstract import AbstractRobotManager
-from smc.control.control_loop_manager import ControlLoopManager
from functools import partial
+from collections import deque
# TODO:
# 1. change the dimensions so that they make sense,
@@ -19,7 +21,7 @@ from functools import partial
# as the uncostrained system
-def getDMPArgs(parser):
+def getDMPArgs(parser: argparse.ArgumentParser) -> argparse.ArgumentParser:
#############################
# dmp specific arguments #
#############################
@@ -60,7 +62,7 @@ def getDMPArgs(parser):
class DMP:
- def __init__(self, trajectory, k=100, d=20, a_s=1, n_bfs=100):
+ def __init__(self, trajectory: str | np.ndarray, k=100, d=20, a_s=1, n_bfs=100):
# TODO load the trajectory here
# and then allocate all these with zeros of appopriate length
# as this way they're basically declared twice
@@ -71,17 +73,18 @@ class DMP:
# this formulation: tau * z_dot = k * (g - y) - d * z + f(x) = h (z, y, s)
# i.e. k = beta_z * alpha_z
# d = alpha_z
- self.k = k
- self.d = d
+ self.k: int = k
+ self.d: int = d
self.a_s = a_s
- self.n_bfs = n_bfs
+ self.n_bfs: int = n_bfs
# trajectory parameters
self.n = 0 # n of dofs
- self.y0 = None # initial position
- self.tau0 = None # final time
- self.g = None # goal positions
- self.tau = None # scaling factor, updated online to ensure traj is followed
+ self.y0: np.ndarray = None # initial position
+ self.tau0: float = None # final time
+ self.g: np.ndarray = None # goal positions
+ # scaling factor, updated online to ensure traj is followed
+ self.tau: float = None
# initialize basis functions for LWR
self.w = None # weights of basis functions
@@ -91,9 +94,9 @@ class DMP:
# state
self.x = None
self.theta = None
- self.pos = None # position
- self.vel = None # velocity
- self.acc = None # acceleration
+ self.pos: np.ndarray = None # position
+ self.vel: np.ndarray = None # velocity
+ self.acc: np.ndarray = None # acceleration
# desired path
self.path = None
@@ -113,7 +116,7 @@ class DMP:
self.time = self.time.reshape(1, len(self.time))
self.y = np.array(trajectory[:, 1:]).T
- def load_trajectory(self, trajectory):
+ def load_trajectory(self, trajectory: np.ndarray):
# load trajectory. this is just joint positions.
self.time = trajectory[:, 0]
self.time = self.time.reshape(1, len(self.time))
@@ -314,7 +317,14 @@ class TCVelAccConstrained:
return taud
-def controlLoopDMP(args, robot: AbstractRobotManager, dmp: DMP, tc, i, past_data):
+def controlLoopDMP(
+ args: argparse.Namespace,
+ robot: AbstractRobotManager,
+ dmp: DMP,
+ tc: NoTC | TCVelAccConstrained,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
controlLoopDMP
-----------------------------
@@ -344,15 +354,17 @@ def controlLoopDMP(args, robot: AbstractRobotManager, dmp: DMP, tc, i, past_data
return breakFlag, save_past_dict, log_item
-def followDMP(args, robot, qs: np.ndarray, tau0):
+def followDMP(
+ args: argparse.Namespace, robot: AbstractRobotManager, qs: np.ndarray, tau0: float
+) -> None:
t = np.linspace(0, tau0, len(qs)).reshape((len(qs), 1))
joint_trajectory = np.hstack((t, qs))
dmp = DMP(joint_trajectory)
if not args.temporal_coupling:
tc = NoTC()
else:
- v_max_ndarray = np.ones(robot.n_arm_joints) * robot.max_qd
- a_max_ndarray = np.ones(robot.n_arm_joints) * args.acceleration
+ v_max_ndarray = np.ones(robot.nq) * robot._max_v
+ a_max_ndarray = np.ones(robot.nq) * args.acceleration
tc = TCVelAccConstrained(
args.gamma_nominal, args.gamma_a, v_max_ndarray, a_max_ndarray, args.eps_tc
)
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index 93af0072c05e71eadc25f2c521e0de1d2ae3e59b..c42b1420c140efb9581c6add0ccade6572335cca 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -138,7 +138,7 @@ class AbstractRobotManager(abc.ABC):
return self._q.copy()
@property
- def nq(self):
+ def nq(self) -> int:
return self.model.nq
@property
@@ -146,7 +146,7 @@ class AbstractRobotManager(abc.ABC):
return self._v.copy()
@property
- def nv(self):
+ def nv(self) -> int:
return self.model.nv
# _updateQ and _updateV could be put into getters and setters of q and v respectively.
diff --git a/python/smc/util/calib_board_hacks.py b/python/smc/util/calib_board_hacks.py
index 6282e8a7f98362178c3e806a6197f02abf89e02d..e214412b5b9d3c51105fbb702585d37538ef70dd 100644
--- a/python/smc/util/calib_board_hacks.py
+++ b/python/smc/util/calib_board_hacks.py
@@ -1,14 +1,13 @@
+from smc.robots.interfaces.force_torque_sensor_interface import ForceTorqueOnSingleArmWrist
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveL, moveUntilContact
+from smc.control.freedrive import freedriveUntilKeyboard
+
+import argparse
+import pickle
import pinocchio as pin
import numpy as np
import time
import copy
-from ur_simple_control.managers import RobotManager
-from ur_simple_control.clik.clik import moveL, moveUntilContact
-from ur_simple_control.basics.basics import freedriveUntilKeyboard
-# used to deal with freedrive's infinite while loop
-import threading
-import argparse
-import pickle
"""
general
@@ -22,7 +21,7 @@ i.e. the normal vector to the plane.
Returns R because that's what's needed to construct the hom transf. mat.
"""
-def getBoardCalibrationArgs(parser):
+def getBoardCalibrationArgs(parser: argparse.ArgumentParser)-> argparse.ArgumentParser:
parser.add_argument('--board-width', type=float, \
help="width of the board (in meters) the robot will write on", \
default=0.30)
@@ -36,7 +35,7 @@ def getBoardCalibrationArgs(parser):
return parser
-def fitNormalVector(positions):
+def fitNormalVector(positions:list[np.ndarray]) -> np.ndarray:
"""
fitNormalVector
----------------
@@ -67,7 +66,7 @@ def fitNormalVector(positions):
print("n_linearly_weighted is singular bruh")
return n_non_weighted
-def constructFrameFromNormalVector(R_initial_estimate, n):
+def constructFrameFromNormalVector(R_initial_estimate:np.ndarray, n:np.ndarray) -> np.ndarray:
"""
constructFrameFromNormalVector
----------------------------------
@@ -93,7 +92,7 @@ def constructFrameFromNormalVector(R_initial_estimate, n):
print(*R, sep=',\n')
return R
-def handleUserToHandleTCPPose(args, robot):
+def handleUserToHandleTCPPose(args:argparse.Namespace, robot:ForceTorqueOnSingleArmWrist)-> None:
"""
handleUserToHandleTCPPose
-----------------------------
@@ -163,7 +162,7 @@ def handleUserToHandleTCPPose(args, robot):
print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
continue
-def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_tests):
+def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist, plane_width:float, plane_height:float, n_tests:int) -> pin.SE3, np.ndarray:
"""
calibratePlane
--------------
@@ -174,14 +173,14 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
handleUserToHandleTCPPose(args, robot)
if args.pinocchio_only:
robot._step()
- q_init = robot.getQ()
- Mtool = robot.getT_w_e()
+ q_init = robot.q
+ T_w_e = robot.T_w_e
- init_pose = copy.deepcopy(Mtool)
+ init_pose = copy.deepcopy(T_w_e)
new_pose = copy.deepcopy(init_pose)
- R_initial_estimate = Mtool.rotation.copy()
- print("initial pose estimate:", Mtool)
+ R_initial_estimate = T_w_e.rotation.copy()
+ print("initial pose estimate:", T_w_e)
R = R_initial_estimate.copy()
go_away_from_plane_transf = pin.SE3.Identity()
@@ -203,15 +202,15 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
moveUntilContact(args, robot, speed)
# no step because this isn't wrapped by controlLoopManager
robot._step()
- q = robot.getQ()
- T_w_e = robot.getT_w_e()
+ q = robot.q
+ T_w_e = robot.T_w_e
print("pin:", *T_w_e.translation.round(4), \
*pin.rpy.matrixToRpy(T_w_e.rotation).round(4))
# print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
positions.append(copy.deepcopy(T_w_e.translation))
if i < n_tests -1:
- current_pose = robot.getT_w_e()
+ current_pose = robot.T_w_e
# go back up
new_pose = current_pose.act(go_away_from_plane_transf)
moveL(args, robot, new_pose)
@@ -238,7 +237,7 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
print("finished estimating R")
- current_pose = robot.getT_w_e()
+ current_pose = robot.T_w_e
new_pose = current_pose.copy()
# go back up
new_pose = new_pose.act(go_away_from_plane_transf)
@@ -259,15 +258,15 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
moveUntilContact(args, robot, speed)
- q = robot.getQ()
- pin.forwardKinematics(robot.model, robot.data, np.array(q))
- Mtool = robot.getT_w_e(q_given=q)
- translation = Mtool.translation.copy()
+ q = robot.q
+ robot.forwardKinematics()
+ T_w_e = robot.computeT_w_e(q)
+ translation = T_w_e.translation.copy()
print("got translation vector, it's:", translation)
moveL(args, robot, new_pose)
- q = robot.getQ()
+ q = robot.q
init_q = copy.deepcopy(q)
print("went back up, saved this q as initial q")