diff --git a/examples/drawing/drawing_ctrl_loop.py b/examples/drawing/drawing_ctrl_loop.py
new file mode 100644
index 0000000000000000000000000000000000000000..432db383ba15a059816c02733f760e64d5b0b182
--- /dev/null
+++ b/examples/drawing/drawing_ctrl_loop.py
@@ -0,0 +1,146 @@
+from smc.robots.interfaces.force_torque_sensor_interface import (
+ ForceTorqueOnSingleArmWrist,
+)
+from smc.control.dmp.dmp import (
+ DMP,
+ NoTC,
+ TCVelAccConstrained,
+)
+from smc.control.cartesian_space import (
+ moveL,
+ compliantMoveL,
+)
+from smc.control import ControlLoopManager
+
+import pinocchio as pin
+import numpy as np
+from argparse import Namespace
+from functools import partial
+from collections import deque
+
+
+def controlLoopWriting(
+ args: Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+ dmp: DMP,
+ tc: NoTC | TCVelAccConstrained,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ controlLoopWriting
+ -----------------------
+ dmp reference on joint path + compliance
+ """
+ breakFlag = False
+ save_past_dict = {}
+ log_item = {}
+ dmp.step(robot.dt)
+ # temporal coupling step
+ tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
+ dmp.set_tau(tau_dmp)
+ q = robot.q
+ Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.0]))
+
+ wrench = robot.wrench
+ save_past_dict["wrench"] = wrench.copy()
+ # rolling average
+ # wrench = np.average(np.array(past_data['wrench']), axis=0)
+
+ # first-order low pass filtering instead
+ # beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
+ # wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(
+ np.array(past_data["wrench"]), axis=0
+ )
+
+ wrench = Z @ wrench
+ J = robot.getJacobian()
+ # get joint
+ tau = J.T @ wrench
+ tau = tau.reshape((-1, 1))
+ # compute control law:
+ # - feedforward the velocity and the force reading
+ # - feedback the position
+ v_cmd = dmp.vel + args.kp * (dmp.pos - q.reshape((-1, 1))) + args.alpha * tau
+ v_cmd = v_cmd.reshape((-1,))
+
+ # tau0 is the minimum time needed for dmp
+ # 500 is the frequency
+ # so we need tau0 * 500 iterations minimum
+ if (np.linalg.norm(dmp.vel) < 0.01) and (i > int(args.tau0 * 500)):
+ breakFlag = True
+ v_cmd = np.zeros(robot.nv)
+
+ # immediatelly stop if something weird happened (some non-convergence)
+ if np.isnan(v_cmd[0]):
+ print("GO NAN FROM INTO VEL_CMD!!! EXITING!!")
+ breakFlag = True
+ v_cmd = np.zeros(robot.nv)
+
+ robot.sendVelocityCommand(v_cmd)
+ log_item["qs"] = robot.q
+ log_item["dmp_qs"] = dmp.pos.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,))
+
+ return breakFlag, save_past_dict, log_item
+
+
+def write(
+ joint_trajectory: np.ndarray,
+ args: Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+) -> None:
+ dmp = DMP(joint_trajectory, a_s=1.0)
+ if not args.temporal_coupling:
+ tc = NoTC()
+ else:
+ 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
+ )
+
+ print("going to starting write position")
+ dmp.step(1 / 500)
+ first_q = dmp.pos.reshape((6,))
+ # move to initial pose
+ 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
+ T_w_goal = T_w_goal.act(go_away_from_plane_transf)
+ if not args.board_wiping:
+ compliantMoveL(T_w_goal, args, robot)
+ else:
+ 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.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.nv),
+ }
+ # moveJ(args, robot, dmp.pos.reshape((6,)))
+ controlLoop = partial(controlLoopWriting, args, robot, dmp, tc)
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ # and now we can actually run
+ loop_manager.run()
+
+ print("move a bit back")
+ 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(goal, args, robot)
diff --git a/examples/drawing/drawing_from_input_drawing.py b/examples/drawing/drawing_from_input_drawing.py
index 4f9a66a38cc0a7b297dd21b418769706f10a1d3f..a6cd2d9e666ba1ce4e454e59e0e16275e48dd06e 100644
--- a/examples/drawing/drawing_from_input_drawing.py
+++ b/examples/drawing/drawing_from_input_drawing.py
@@ -1,27 +1,18 @@
-from utils import getArgs
+from utils import getArgsForDrawing
from get_marker import getMarker
from find_marker_offset import findMarkerOffset
+from drawing_ctrl_loop import write
from smc.robots.interfaces.force_torque_sensor_interface import (
ForceTorqueOnSingleArmWrist,
)
from smc.util.draw_path import drawPath
-from smc.control.dmp.dmp import (
- DMP,
- NoTC,
- TCVelAccConstrained,
-)
from smc.control.cartesian_space.ik_solvers import getIKSolver
-from smc.control.cartesian_space import (
- moveL,
- compliantMoveL,
-)
from smc.path_generation.cartesian_to_joint_space_path_mapper import (
clikCartesianPathIntoJointPath,
)
from smc.util.map2DPathTo3DPlane import map2DPathTo3DPlane
from smc import getRobotFromArgs
-from smc.control.control_loop_manager import ControlLoopManager
from smc.util.calib_board_hacks import (
calibratePlane,
)
@@ -29,141 +20,7 @@ from smc.util.calib_board_hacks import (
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 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
- -----------------------
- dmp reference on joint path + compliance
- """
- breakFlag = False
- save_past_dict = {}
- log_item = {}
- dmp.step(robot.dt)
- # temporal coupling step
- tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
- dmp.set_tau(tau_dmp)
- q = robot.q
- Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.0]))
-
- wrench = robot.wrench
- save_past_dict["wrench"] = wrench.copy()
- # rolling average
- # wrench = np.average(np.array(past_data['wrench']), axis=0)
-
- # first-order low pass filtering instead
- # beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
- # wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- wrench = args.beta * wrench + (1 - args.beta) * np.average(
- np.array(past_data["wrench"]), axis=0
- )
-
- wrench = Z @ wrench
- J = robot.getJacobian()
- # get joint
- tau = J.T @ wrench
- 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.reshape((-1, 1))) + args.alpha * tau
- robot.sendVelocityCommand(vel_cmd)
-
- # tau0 is the minimum time needed for dmp
- # 500 is the frequency
- # so we need tau0 * 500 iterations minimum
- if (np.linalg.norm(dmp.vel) < 0.01) and (i > int(args.tau0 * 500)):
- breakFlag = True
- # immediatelly stop if something weird happened (some non-convergence)
- if np.isnan(vel_cmd[0]):
- print("GO NAN FROM INTO VEL_CMD!!! EXITING!!")
- breakFlag = True
-
- # log what you said you'd log
- # TODO fix the q6 situation (hide this)
- log_item["qs"] = robot.q
- log_item["dmp_qs"] = dmp.pos.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,))
-
- return breakFlag, save_past_dict, log_item
-
-
-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.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
- )
-
- print("going to starting write position")
- dmp.step(1 / 500)
- first_q = dmp.pos.reshape((6,))
- # move to initial pose
- 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
- T_w_goal = T_w_goal.act(go_away_from_plane_transf)
- if not args.board_wiping:
- compliantMoveL(T_w_goal, args, robot)
- else:
- 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.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.nv),
- }
- # moveJ(args, robot, dmp.pos.reshape((6,)))
- controlLoop = partial(controlLoopWriting, args, robot, dmp, tc)
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- # and now we can actually run
- loop_manager.run()
-
- print("move a bit back")
- 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(goal, args, robot)
if __name__ == "__main__":
@@ -171,7 +28,7 @@ if __name__ == "__main__":
#######################################################################
# software setup #
#######################################################################
- args = getArgs()
+ args = getArgsForDrawing()
if not args.board_wiping:
assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
args.mm_into_board = args.mm_into_board / 1000
@@ -183,9 +40,7 @@ if __name__ == "__main__":
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._q = np.array([1.32, -1.40, -1.27, -1.157, 1.76, -0.238]) + rand_pertb
robot._step()
#######################################################################
@@ -200,33 +55,28 @@ if __name__ == "__main__":
matplotlib.use("qtagg")
else:
if not args.board_wiping:
- pixel_path_file_path = "./path_in_pixels.csv"
+ pixel_path_file_path = "./parameters/path_in_pixels.csv"
pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=",")
else:
- pixel_path_file_path = "./wiping_path.csv_save"
+ pixel_path_file_path = "./parameters/wiping_path.csv_save"
pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=",")
# do calibration if specified
- if args.calibration:
+ if args.calibrate_plane:
plane_pose, q_init = calibratePlane(
args, robot, args.board_width, args.board_height, args.n_calibration_tests
)
print("finished calibration")
else:
print("using existing plane calibration")
- file = open("./plane_pose.pickle_save", "rb")
+ file = open("./parameters/plane_pose.pickle_save", "rb")
plane_calib_dict = pickle.load(file)
file.close()
plane_pose = plane_calib_dict["plane_top_left_pose"]
q_init = plane_calib_dict["q_init"]
- # stupid fix dw about this it's supposed to be 0.0 on the gripper
- # because we don't use that actually
- q_init[-1] = 0.0
- q_init[-2] = 0.0
# make the path 3D
path_points_3D = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
if args.pick_up_marker:
- # raise NotImplementedError("sorry")
getMarker(args, robot)
# marker moves between runs, i change markers etc,
@@ -250,7 +100,7 @@ if __name__ == "__main__":
)
# create a joint space trajectory based on the 3D path
- if args.draw_new or args.calibration or args.find_marker_offset:
+ if args.draw_new or args.calibrate_plane or args.find_marker_offset:
path = []
for i in range(len(path_points_3D)):
path_pose = pin.SE3.Identity()
@@ -264,9 +114,9 @@ if __name__ == "__main__":
it has to look reasonable, otherwise we can't run it!
"""
)
- clikController = getIKSolver(args, robot)
+ ik_solver = getIKSolver(args, robot)
joint_trajectory = clikCartesianPathIntoJointPath(
- args, robot, path, clikController, q_init, plane_pose
+ ik_solver, path, q_init, args.tau0, args, robot
)
if args.viz_test_path:
answer = input("did the movement of the manipulator look reasonable? [Y/n]")
@@ -278,7 +128,7 @@ if __name__ == "__main__":
else:
answer = True
else:
- joint_trajectory_file_path = "./joint_trajectory.csv"
+ joint_trajectory_file_path = "./parameters/joint_trajectory.csv"
joint_trajectory = np.genfromtxt(joint_trajectory_file_path, delimiter=",")
if answer:
diff --git a/examples/drawing/get_marker.py b/examples/drawing/get_marker.py
index aa6bd57be7628cb0be3d97249c76cf8b8468b168..67a0d986f8dd6e12aaa224ae6437aa0b81ba407f 100644
--- a/examples/drawing/get_marker.py
+++ b/examples/drawing/get_marker.py
@@ -16,7 +16,17 @@ def getMarker(args: Namespace, robot: SingleArmInterface) -> None:
if not, generate a different joint trajectory for your situation.
"""
# load traj
- file = open("./data/from_writing_to_handover.pickle_save", "rb")
+ try:
+ file = open("./data/from_writing_to_handover.pickle_save", "rb")
+ except FileNotFoundError:
+ print(
+ "there is not file on path './data/from_writing_to_handover.pickle_save'!"
+ )
+ print("(1) check from which directory you're running the script")
+ print(
+ "(2) if you don't wave a from_writing_to_handover.pickle_save, you can create it by teaching by demostration available within freedrive_v2.0 found in convenience_tool_box (details on it are there)"
+ )
+ exit()
point_dict = pickle.load(file)
file.close()
#####################
diff --git a/examples/drawing/parameters/plane_pose.pickle_save b/examples/drawing/parameters/plane_pose.pickle_save
index d664870c456e2d2ec0674aab04531818a00a3430..f1d2e30606528e2ec935b3629cd202fd0b78db0a 100644
Binary files a/examples/drawing/parameters/plane_pose.pickle_save and b/examples/drawing/parameters/plane_pose.pickle_save differ
diff --git a/examples/drawing/parameters/plane_pose_sim.pickle_save b/examples/drawing/parameters/plane_pose_sim.pickle_save
new file mode 100644
index 0000000000000000000000000000000000000000..f1d2e30606528e2ec935b3629cd202fd0b78db0a
Binary files /dev/null and b/examples/drawing/parameters/plane_pose_sim.pickle_save differ
diff --git a/examples/drawing/parameters/wiping_path.csv_save b/examples/drawing/parameters/wiping_path.csv_save
deleted file mode 100644
index 3971e66aef355df0007209be0e714e1b957757d3..0000000000000000000000000000000000000000
--- a/examples/drawing/parameters/wiping_path.csv_save
+++ /dev/null
@@ -1,546 +0,0 @@
-0.15129,0.89211
-0.15129,0.89211
-0.15277,0.89211
-0.15573,0.89078
-0.15870,0.88145
-0.15870,0.87346
-0.15870,0.85614
-0.15870,0.83217
-0.15722,0.81618
-0.15573,0.80553
-0.15129,0.78288
-0.14981,0.76557
-0.14685,0.74958
-0.14536,0.73893
-0.14240,0.72294
-0.13944,0.70962
-0.13796,0.69897
-0.13648,0.68831
-0.13500,0.67899
-0.13500,0.67366
-0.13500,0.66567
-0.13500,0.65634
-0.13351,0.64436
-0.13351,0.62970
-0.13351,0.61772
-0.13351,0.60706
-0.13203,0.57642
-0.13203,0.56044
-0.13055,0.54579
-0.13055,0.53646
-0.13055,0.52181
-0.13055,0.50583
-0.13055,0.48984
-0.13055,0.47786
-0.13055,0.46720
-0.13203,0.45122
-0.13203,0.43656
-0.13351,0.42058
-0.13351,0.40593
-0.13500,0.39261
-0.13648,0.37130
-0.13796,0.33666
-0.13796,0.32601
-0.13796,0.31002
-0.13796,0.29537
-0.13796,0.28338
-0.13796,0.27273
-0.13648,0.26474
-0.13648,0.23410
-0.13648,0.22078
-0.13648,0.21012
-0.13648,0.20346
-0.13648,0.19814
-0.13648,0.19414
-0.13648,0.19281
-0.13648,0.19014
-0.13796,0.17816
-0.13796,0.17016
-0.13796,0.16084
-0.13796,0.15285
-0.13796,0.14219
-0.13796,0.13953
-0.15277,0.13953
-0.16610,0.13953
-0.18240,0.13820
-0.19277,0.13686
-0.20462,0.13553
-0.22388,0.13154
-0.23573,0.13020
-0.24758,0.12754
-0.25795,0.12488
-0.26388,0.12354
-0.26684,0.12354
-0.27129,0.12354
-0.27277,0.12354
-0.27277,0.15152
-0.27277,0.17416
-0.26980,0.23543
-0.26240,0.26607
-0.25795,0.30336
-0.25499,0.33400
-0.25647,0.36996
-0.26092,0.38861
-0.26536,0.41658
-0.26832,0.43923
-0.26980,0.45521
-0.26980,0.47786
-0.26980,0.53114
-0.26980,0.55778
-0.26980,0.58308
-0.27129,0.59774
-0.27129,0.60972
-0.27129,0.61905
-0.27129,0.62171
-0.27129,0.62304
-0.27129,0.62970
-0.26684,0.68165
-0.26684,0.69497
-0.26684,0.71229
-0.26684,0.72028
-0.26684,0.72428
-0.26684,0.72827
-0.26684,0.73227
-0.26684,0.73760
-0.26684,0.74692
-0.26832,0.76024
-0.26980,0.77489
-0.26980,0.79088
-0.27129,0.80819
-0.27277,0.82151
-0.27277,0.82950
-0.27277,0.83616
-0.27277,0.84149
-0.27277,0.84549
-0.27277,0.85082
-0.27277,0.85748
-0.27277,0.86547
-0.27277,0.87346
-0.27277,0.87746
-0.27277,0.88012
-0.27277,0.88278
-0.27277,0.88412
-0.27277,0.88545
-0.27573,0.88944
-0.28017,0.89344
-0.28462,0.89610
-0.29203,0.89877
-0.29795,0.90143
-0.30388,0.90276
-0.31128,0.90410
-0.31869,0.90410
-0.32462,0.90410
-0.33054,0.90410
-0.33647,0.90410
-0.34091,0.90276
-0.34536,0.90143
-0.34980,0.90010
-0.35573,0.89877
-0.36165,0.89744
-0.37054,0.89744
-0.37499,0.89744
-0.38239,0.89744
-0.38684,0.89744
-0.38832,0.89744
-0.39128,0.89744
-0.39424,0.89744
-0.39573,0.89744
-0.39721,0.89744
-0.39869,0.89744
-0.39869,0.89610
-0.40165,0.89344
-0.40461,0.86680
-0.40461,0.85082
-0.40461,0.83483
-0.40461,0.82418
-0.40461,0.80952
-0.40461,0.78688
-0.40461,0.77356
-0.40461,0.76290
-0.40461,0.75491
-0.40461,0.73760
-0.40461,0.72294
-0.40461,0.70829
-0.40461,0.70030
-0.40461,0.69098
-0.40461,0.68565
-0.40461,0.67632
-0.40461,0.66434
-0.40758,0.65102
-0.41054,0.64036
-0.41202,0.62704
-0.41350,0.61905
-0.41498,0.60839
-0.41647,0.59374
-0.41795,0.56044
-0.41647,0.55112
-0.41647,0.54446
-0.41498,0.53513
-0.41202,0.52847
-0.41054,0.52048
-0.40758,0.51249
-0.40461,0.50183
-0.40165,0.48984
-0.39869,0.48452
-0.39573,0.47786
-0.39276,0.46587
-0.38980,0.46054
-0.38684,0.44988
-0.38387,0.44189
-0.38239,0.43124
-0.38091,0.42458
-0.37943,0.41658
-0.37943,0.41259
-0.37795,0.40593
-0.37795,0.40193
-0.37795,0.39927
-0.37795,0.39261
-0.37795,0.38328
-0.37795,0.37263
-0.37795,0.36197
-0.37943,0.35265
-0.37943,0.34599
-0.37943,0.34199
-0.37943,0.33933
-0.37943,0.33533
-0.37943,0.33000
-0.38091,0.32468
-0.38091,0.31269
-0.38091,0.30203
-0.38239,0.28605
-0.38239,0.27939
-0.38239,0.26340
-0.38387,0.25541
-0.38387,0.24742
-0.38387,0.24209
-0.38387,0.23543
-0.38387,0.23144
-0.38387,0.22611
-0.38536,0.21812
-0.38684,0.21412
-0.38684,0.21012
-0.38684,0.20613
-0.38684,0.20213
-0.38684,0.20213
-0.38684,0.20080
-0.38684,0.19947
-0.38684,0.19814
-0.38980,0.19148
-0.39424,0.18082
-0.40017,0.17150
-0.40313,0.16750
-0.40461,0.16350
-0.40461,0.16217
-0.40461,0.16084
-0.40461,0.15951
-0.40461,0.15818
-0.40461,0.15684
-0.40610,0.15551
-0.40906,0.15152
-0.41054,0.14885
-0.41202,0.14619
-0.41202,0.14619
-0.41202,0.14486
-0.41202,0.14352
-0.41202,0.14219
-0.41498,0.14219
-0.42091,0.14086
-0.43276,0.14086
-0.44313,0.14352
-0.45350,0.14486
-0.46239,0.14752
-0.46980,0.14885
-0.47424,0.14885
-0.48017,0.15018
-0.48461,0.15018
-0.48757,0.15018
-0.48906,0.15152
-0.49054,0.15152
-0.49498,0.15285
-0.49943,0.15418
-0.50387,0.15418
-0.50831,0.15551
-0.51128,0.15551
-0.51276,0.15551
-0.51424,0.15551
-0.51572,0.15551
-0.50980,0.17416
-0.50683,0.19148
-0.50535,0.21678
-0.50387,0.24209
-0.50387,0.25808
-0.50387,0.29004
-0.50387,0.32601
-0.50387,0.34199
-0.50683,0.39527
-0.50683,0.40193
-0.50831,0.43390
-0.50980,0.45122
-0.51128,0.46587
-0.51276,0.47786
-0.51424,0.48851
-0.51572,0.50583
-0.51572,0.52314
-0.51720,0.53646
-0.51868,0.54978
-0.52017,0.56577
-0.52017,0.57909
-0.52017,0.59108
-0.52017,0.61638
-0.52017,0.62837
-0.52017,0.64569
-0.51868,0.65901
-0.51868,0.66966
-0.51868,0.67899
-0.51868,0.68831
-0.51868,0.70163
-0.51868,0.71229
-0.51868,0.72561
-0.52165,0.73760
-0.52461,0.75092
-0.52905,0.76690
-0.53350,0.77889
-0.53498,0.78688
-0.53646,0.79487
-0.53794,0.80020
-0.53794,0.80286
-0.53942,0.80819
-0.53942,0.81352
-0.54091,0.82018
-0.54091,0.82817
-0.54091,0.83217
-0.54091,0.83750
-0.54091,0.84282
-0.54091,0.84416
-0.54091,0.84549
-0.54239,0.85748
-0.54239,0.86547
-0.54239,0.86813
-0.54239,0.87080
-0.54239,0.87612
-0.54239,0.87612
-0.54239,0.87746
-0.54239,0.87879
-0.54239,0.88012
-0.54239,0.88145
-0.54239,0.88278
-0.54239,0.88412
-0.54239,0.88545
-0.54979,0.88944
-0.55720,0.89211
-0.56609,0.89211
-0.57794,0.89211
-0.58831,0.89211
-0.59720,0.89078
-0.60609,0.88944
-0.61794,0.88678
-0.62387,0.88545
-0.62979,0.88412
-0.63720,0.88145
-0.64609,0.87879
-0.65498,0.87612
-0.66090,0.87346
-0.66979,0.87213
-0.67423,0.87080
-0.67720,0.87080
-0.67868,0.87080
-0.68016,0.87080
-0.68164,0.87080
-0.68312,0.87080
-0.68460,0.86946
-0.68460,0.86813
-0.68757,0.86280
-0.68757,0.85215
-0.68460,0.83616
-0.67868,0.80153
-0.67572,0.78155
-0.67423,0.76690
-0.67423,0.71362
-0.67423,0.70030
-0.67423,0.68698
-0.67275,0.66034
-0.67127,0.64436
-0.67127,0.62970
-0.66979,0.61638
-0.66831,0.58575
-0.66683,0.56177
-0.66683,0.55112
-0.66683,0.52847
-0.66831,0.49650
-0.66831,0.48851
-0.66535,0.47120
-0.66238,0.44589
-0.65794,0.42724
-0.65498,0.41658
-0.65053,0.39128
-0.64905,0.36464
-0.64609,0.33800
-0.64461,0.32201
-0.64312,0.30603
-0.64164,0.29138
-0.64164,0.28205
-0.63868,0.26740
-0.63868,0.23144
-0.63868,0.21945
-0.63868,0.21412
-0.63868,0.20613
-0.63868,0.20213
-0.63868,0.19947
-0.63868,0.19680
-0.63868,0.19014
-0.63868,0.18748
-0.63868,0.18215
-0.63868,0.17949
-0.63868,0.17816
-0.64016,0.17150
-0.64164,0.16617
-0.64312,0.15818
-0.64312,0.15684
-0.64312,0.15418
-0.64312,0.15285
-0.64312,0.15152
-0.64312,0.15018
-0.64312,0.14885
-0.67127,0.14885
-0.68757,0.14619
-0.69497,0.14352
-0.71275,0.13953
-0.73794,0.13287
-0.75571,0.13020
-0.76608,0.12887
-0.77349,0.12754
-0.77942,0.12754
-0.78682,0.12754
-0.79127,0.12754
-0.79867,0.12754
-0.80608,0.12754
-0.81349,0.12754
-0.81645,0.12754
-0.81645,0.12754
-0.81793,0.12754
-0.81941,0.12754
-0.82090,0.12754
-0.82238,0.12754
-0.82682,0.13154
-0.83275,0.15551
-0.83275,0.16617
-0.82978,0.19547
-0.82534,0.22078
-0.82238,0.23543
-0.81941,0.27006
-0.81941,0.29271
-0.81941,0.30869
-0.81941,0.33400
-0.81941,0.35132
-0.81941,0.36464
-0.82090,0.37796
-0.82090,0.39927
-0.82238,0.42191
-0.82238,0.43790
-0.82534,0.46454
-0.82534,0.48052
-0.82534,0.48718
-0.82534,0.48984
-0.82534,0.49118
-0.82534,0.50982
-0.82534,0.52448
-0.82534,0.59774
-0.82386,0.61905
-0.82238,0.63503
-0.82238,0.65235
-0.82238,0.66700
-0.82238,0.67766
-0.82238,0.69098
-0.82238,0.69897
-0.82238,0.71495
-0.82238,0.73227
-0.82238,0.74692
-0.82238,0.76024
-0.82386,0.78022
-0.82386,0.78422
-0.82534,0.79620
-0.82682,0.80153
-0.82830,0.80952
-0.82978,0.81618
-0.83127,0.82284
-0.83127,0.82817
-0.83127,0.82950
-0.83127,0.83084
-0.83127,0.83217
-0.83127,0.83350
-0.83423,0.83750
-0.83719,0.84149
-0.83867,0.84282
-0.84015,0.84416
-0.84164,0.84549
-0.84312,0.84682
-0.84756,0.84815
-0.85497,0.84815
-0.86089,0.84815
-0.86386,0.84948
-0.86978,0.84815
-0.87719,0.84815
-0.88312,0.84815
-0.89200,0.84815
-0.89645,0.84815
-0.89793,0.84815
-0.89941,0.84815
-0.90089,0.84815
-0.90237,0.84815
-0.90386,0.84815
-0.90534,0.84682
-0.90682,0.84682
-0.90830,0.84682
-0.90978,0.84682
-0.91126,0.84549
-0.91274,0.84549
-0.91571,0.84016
-0.91867,0.83084
-0.92163,0.81885
-0.92311,0.80819
-0.92311,0.79088
-0.92311,0.75624
-0.92311,0.73760
-0.92311,0.71895
-0.92756,0.65768
-0.92756,0.64302
-0.93052,0.61505
-0.93200,0.57776
-0.93200,0.56976
-0.93052,0.55511
-0.93052,0.53247
-0.92904,0.51249
-0.92756,0.50050
-0.92460,0.48452
-0.92460,0.45122
-0.92460,0.44056
-0.92460,0.43124
-0.92460,0.42058
-0.92460,0.40326
-0.92460,0.37396
-0.92460,0.36464
-0.92460,0.34998
-0.92460,0.32601
-0.92460,0.30869
-0.92460,0.29937
-0.92460,0.29404
-0.92311,0.28738
-0.92163,0.28472
-0.92163,0.28205
-0.92015,0.27672
-0.91423,0.24342
-0.91423,0.23277
-0.91571,0.21678
-0.92163,0.18348
-0.92311,0.17150
-0.92608,0.16217
-0.92608,0.15818
-0.92608,0.15551
-0.92608,0.15418
-0.92608,0.15418
-0.92608,0.15285
-0.92608,0.15018
-0.92608,0.14885
-0.92608,0.14752
-0.92608,0.14619
-0.92608,0.14619
diff --git a/examples/drawing/technical_report/drawing_from_input.tex b/examples/drawing/technical_report/drawing_from_input.tex
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/examples/drawing/utils.py b/examples/drawing/utils.py
index 407c0f1042a7c33b469370e7ef3dcb23ca3fbe14..5bc7bec6aa99be2f49dfd44fac70520f5aac5734 100644
--- a/examples/drawing/utils.py
+++ b/examples/drawing/utils.py
@@ -2,31 +2,23 @@ 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:
+def getArgsForDrawing() -> 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,
diff --git a/examples/impedance/point_impedance_control.py b/examples/impedance/point_impedance_control.py
index a21977556931b67cd410ca8ce19c9257d74ddc25..7d15a642bec474b559283e0df0ef5ce18e5215b9 100644
--- a/examples/impedance/point_impedance_control.py
+++ b/examples/impedance/point_impedance_control.py
@@ -1,48 +1,32 @@
+from smc.robots.interfaces.force_torque_sensor_interface import (
+ ForceTorqueOnSingleArmWrist,
+)
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.control import ControlLoopManager
+from smc.control.cartesian_space import (
+ getClikArgs,
+ getIKSolver,
+)
+
import pinocchio as pin
import numpy as np
import copy
import argparse
from functools import partial
-from smc.clik.clik import (
- getClikArgs,
- getClikController,
- moveL,
- moveUntilContact,
-)
-from smc.managers import (
- getMinimalArgParser,
- ControlLoopManager,
- RobotManager,
-)
-from smc.basics.basics import moveJPI
+from typing import Any, Callable
+from collections import deque
def getArgs():
parser = getMinimalArgParser()
parser = getClikArgs(parser)
- parser.add_argument(
- "--kp",
- type=float,
- help="proportial control constant for position errors",
- default=1.0,
- )
- parser.add_argument(
- "--kv", type=float, help="damping in impedance control", default=0.001
- )
parser.add_argument(
"--cartesian-space-impedance",
action=argparse.BooleanOptionalAction,
help="is the impedance computed and added in cartesian or in joint space",
default=False,
)
- parser.add_argument(
- "--z-only",
- action=argparse.BooleanOptionalAction,
- help="whether you have general impedance or just ee z axis",
- default=False,
- )
- # argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
@@ -53,16 +37,20 @@ def controller():
# control loop to be passed to ControlLoopManager
-def controlLoopPointImpedance(
- args, q_init, controller, robot: RobotManager, i, past_data
-):
+def JointSpacePointImpedanceControlLoop(
+ controller: Any,
+ q_init: np.ndarray,
+ args: argparse.Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
log_item = {}
- q = robot.getQ()
- Mtool = robot.getT_w_e()
- wrench = robot.getWrench()
+ q = robot.q
+ wrench = robot.wrench
log_item["wrench_raw"] = wrench.reshape((6,))
# deepcopy for good coding practise (and correctness here)
save_past_dict["wrench"] = copy.deepcopy(wrench)
@@ -75,104 +63,87 @@ def controlLoopPointImpedance(
np.array(past_data["wrench"]), axis=0
)
if not args.z_only:
- Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
+ Z = np.diag(np.array([1.0, 1.0, 1.0, 5.0, 5.0, 5.0]))
else:
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
- # Z = np.diag(np.ones(6))
wrench = Z @ wrench
- # this jacobian might be wrong
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
- dq = robot.getQd()[:6].reshape((6, 1))
- # get joint
+ J = robot.getJacobian()
tau = J.T @ wrench
- # if i % 25 == 0:
- # print(*tau.round(1))
tau = tau[:6].reshape((6, 1))
# compute control law:
# - feedback the position
# kv is not needed if we're running velocity control
- vel_cmd = (
- args.kp * (q_init[:6].reshape((6, 1)) - q[:6].reshape((6, 1)))
- + args.alpha * tau
- )
- # vel_cmd = np.zeros(6)
- robot.sendQd(vel_cmd)
-
+ v_cmd = args.kp * (q_init.reshape((-1, 1)) - q.reshape((-1, 1))) + args.alpha * tau
# immediatelly stop if something weird happened (some non-convergence)
- if np.isnan(vel_cmd[0]):
+ if np.isnan(v_cmd[0]):
breakFlag = True
+ v_cmd = np.zeros(robot.nv)
+ robot.sendVelocityCommand(v_cmd)
- # log what you said you'd log
- # TODO fix the q6 situation (hide this)
- log_item["qs"] = q[:6].reshape((6,))
- log_item["dqs"] = dq.reshape((6,))
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
log_item["wrench_used"] = wrench.reshape((6,))
-
return breakFlag, save_past_dict, log_item
def controlLoopCartesianPointImpedance(
- args, Mtool_init, clik_controller, robot: RobotManager, i, past_data
-):
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ Mtool_init: pin.SE3,
+ args: argparse.Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
+
breakFlag = False
- # TODO rename this into something less confusing
save_past_dict = {}
log_item = {}
- q = robot.getQ()
- Mtool = robot.getT_w_e()
- wrench = robot.getWrench()
+ T_w_e = robot.T_w_e
+ wrench = robot.wrench
log_item["wrench_raw"] = wrench.reshape((6,))
save_past_dict["wrench"] = copy.deepcopy(wrench)
- # wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+
wrench = args.beta * wrench + (1 - args.beta) * np.average(
np.array(past_data["wrench"]), axis=0
)
# good generic values
- # Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
- # but let's stick to the default for now
if not args.z_only:
Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
else:
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
- # Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
- # Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
-
wrench = Z @ wrench
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
-
- SEerror = Mtool.actInv(Mtool_init)
+ SEerror = T_w_e.actInv(Mtool_init)
err_vector = pin.log6(SEerror).vector
v_cartesian_body_cmd = args.kp * err_vector + args.alpha * wrench
- vel_cmd = clik_controller(J, v_cartesian_body_cmd)
- robot.sendQd(vel_cmd)
+ J = robot.getJacobian()
+ v_cmd = ik_solver(J, v_cartesian_body_cmd)
# immediatelly stop if something weird happened (some non-convergence)
- if np.isnan(vel_cmd[0]):
+ if np.isnan(v_cmd[0]):
breakFlag = True
+ v_cmd = np.zeros(robot.nv)
+ robot.sendVelocityCommand(v_cmd)
- dq = robot.getQd()[:6].reshape((6, 1))
- # log what you said you'd log
- # TODO fix the q6 situation (hide this)
- log_item["qs"] = q[:6].reshape((6,))
- log_item["dqs"] = dq.reshape((6,))
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
log_item["wrench_used"] = wrench.reshape((6,))
-
return breakFlag, save_past_dict, log_item
if __name__ == "__main__":
args = getArgs()
- robot = RobotManager(args)
- clikController = getClikController(args, robot)
+ robot = getRobotFromArgs(args)
+ assert issubclass(robot.__class__, ForceTorqueOnSingleArmWrist)
+ ik_solver = getIKSolver(args, robot)
- # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+ # NOTE: the weight, TCP and inertial matrix needs to be set on the robot
# you already found an API in rtde_control for this, just put it in initialization
# under using/not-using gripper parameters
- # ALSO NOTE: to use this you need to change the version inclusions in
+ # NOTE: to use this you need to change the version inclusions in
# ur_rtde due to a bug there in the current ur_rtde + robot firmware version
# (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
# here you give what you're saving in the rolling past window
@@ -183,21 +154,21 @@ if __name__ == "__main__":
}
# here you give it it's initial value
log_item = {
- "qs": np.zeros(robot.n_arm_joints),
- "dqs": np.zeros(robot.n_arm_joints),
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
"wrench_raw": np.zeros(6),
"wrench_used": np.zeros(6),
}
- q_init = robot.getQ()
- Mtool_init = robot.getT_w_e()
+ q_init = robot.q
+ T_w_einit = robot.T_w_e
if not args.cartesian_space_impedance:
controlLoop = partial(
- controlLoopPointImpedance, args, q_init, controller, robot
+ JointSpacePointImpedanceControlLoop, controller, q_init, args, robot
)
else:
controlLoop = partial(
- controlLoopCartesianPointImpedance, args, Mtool_init, clikController, robot
+ controlLoopCartesianPointImpedance, ik_solver, T_w_einit, args, robot
)
loop_manager = ControlLoopManager(
@@ -209,8 +180,8 @@ if __name__ == "__main__":
robot.stopRobot()
if args.save_log:
- robot.log_manager.saveLog()
- robot.log_manager.plotAllControlLoops()
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
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 f50ae8319133e851335646956676177938370a2d..402f6b28e63fd5cdca86da896f897bb665d38dfd 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
@@ -63,7 +63,7 @@ def 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
+ assert type(T_w_goal) == pin.SE3
ik_solver = getIKSolver(args, robot)
controlLoop = partial(
EEP2PCtrlLoopTemplate, ik_solver, T_w_goal, controlLoopClik, args, robot
@@ -139,8 +139,8 @@ def moveUntilContactControlLoop(
if np.linalg.norm(wrench[mask]) > args.contact_detecting_force:
print("hit with", np.linalg.norm(wrench[mask]))
breakFlag = True
- robot.sendVelocityCommand(np.zeros(robot.nq))
- if (args.pinocchio_only) and (i > 500):
+ robot.sendVelocityCommand(np.zeros(robot.nv))
+ if (not args.real) and (i > 500):
print("let's say you hit something lule")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
diff --git a/python/smc/control/cartesian_space/utils.py b/python/smc/control/cartesian_space/utils.py
index 4bdbccb15677c19c8ba856d3d3edaa6bb50d34f5..685ed51b98af585f76964fdc5efebb92ea28c5c9 100644
--- a/python/smc/control/cartesian_space/utils.py
+++ b/python/smc/control/cartesian_space/utils.py
@@ -1,7 +1,7 @@
import argparse
-def getClikArgs(parser: argparse.ArgumentParser):
+def getClikArgs(parser: argparse.ArgumentParser) -> argparse.ArgumentParser:
"""
getClikArgs
------------
@@ -61,6 +61,22 @@ def getClikArgs(parser: argparse.ArgumentParser):
parser.add_argument(
"--beta", type=float, help="low-pass filter beta parameter", default=0.01
)
+ parser.add_argument(
+ "--kp",
+ type=float,
+ help="proportial control constant for position errors",
+ default=1.0,
+ )
+ # NOTE: unnecessary on velocity controlled robots
+ parser.add_argument(
+ "--kv", type=float, help="damping in impedance control", default=0.001
+ )
+ parser.add_argument(
+ "--z-only",
+ action=argparse.BooleanOptionalAction,
+ help="whether you have general impedance or just ee z axis",
+ default=False,
+ )
###############################
# path following parameters #
diff --git a/python/smc/control/dmp/dmp.py b/python/smc/control/dmp/dmp.py
index 0737daa6ce0f29413531d4efb53471f9b17d3e9a..62cdc20bdd922cd6886520e460679f7ba3e046db 100644
--- a/python/smc/control/dmp/dmp.py
+++ b/python/smc/control/dmp/dmp.py
@@ -62,7 +62,7 @@ def getDMPArgs(parser: argparse.ArgumentParser) -> argparse.ArgumentParser:
class DMP:
- def __init__(self, trajectory: str | np.ndarray, k=100, d=20, a_s=1, n_bfs=100):
+ def __init__(self, trajectory: str | np.ndarray, k=100, d=20, a_s:float=1.0, 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
diff --git a/python/smc/control/freedrive.py b/python/smc/control/freedrive.py
index 5628e7a91a1c66e530ffaa97905a895c95716f8b..25c8194d5840945a1a10bf931034e4691110d455 100644
--- a/python/smc/control/freedrive.py
+++ b/python/smc/control/freedrive.py
@@ -61,7 +61,7 @@ def freedriveUntilKeyboard(args, robot: SingleArmInterface):
you can save the log from this function and use that,
or type on the keyboard to save specific points (q + T_w_e)
"""
- if args.pinocchio_only:
+ if not args.real:
print(
"""
ideally now you would use some sliders or something,
diff --git a/python/smc/path_generation/cartesian_to_joint_space_path_mapper.py b/python/smc/path_generation/cartesian_to_joint_space_path_mapper.py
index 219ba53d96008caccf098f70c265037c31f1451d..4d82d54cafae1933afeb6bbe6ece21fcb4117503 100644
--- a/python/smc/path_generation/cartesian_to_joint_space_path_mapper.py
+++ b/python/smc/path_generation/cartesian_to_joint_space_path_mapper.py
@@ -1,11 +1,22 @@
-from smc.robots.robotmanager_abstract import AbstractRobotManager
-import pinocchio as pin
+from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc import getRobotFromArgs
+from smc.control.cartesian_space import moveL
+
+from argparse import Namespace
import numpy as np
+from pinocchio import SE3
+import copy
+from functools import partial
def clikCartesianPathIntoJointPath(
- args, robot, path, clikController, q_init, plane_pose
-):
+ ik_solver,
+ path: list[SE3],
+ q_init: np.ndarray,
+ t_final: float,
+ args: Namespace,
+ robot: AbstractRobotManager,
+) -> np.ndarray:
"""
clikCartesianPathIntoJointPath
------------------------------
@@ -20,10 +31,6 @@ def clikCartesianPathIntoJointPath(
return
------
- joint_space_trajectory to follow the given path.
-
- arguments
- ----------
- - path: cartesian path given in task frame
"""
# we don't know how many there will be, so a linked list is
# clearly the best data structure here (instert is o(1) still,
@@ -40,20 +47,21 @@ def clikCartesianPathIntoJointPath(
sim_args.visualizer = False
sim_args.save_log = False # we're not using sim robot outside of this
sim_args.max_iterations = 10000 # more than enough
- sim_robot = AbstractRobotManager(sim_args)
- sim_robot.q = q_init.copy()
+ if type(ik_solver) is partial:
+ sim_args.ik_solver = ik_solver.func.__name__
+ else:
+ sim_args.ik_solver = ik_solver.__name__
+ sim_robot = getRobotFromArgs(sim_args)
+ sim_robot._q = q_init.copy()
sim_robot._step()
for pose in path:
moveL(sim_args, sim_robot, pose)
- # loop logs is a dict, dict keys list preserves input order
- new_q = sim_robot.q.copy()
if args.viz_test_path:
- # look into visualize.py for details on what's available
- T_w_e = sim_robot.getT_w_e()
- robot.updateViz({"q": new_q, "T_w_e": T_w_e, "point": T_w_e.copy()})
+ robot.updateViz(
+ {"q": sim_robot.q, "T_w_e": sim_robot.T_w_e, "point": sim_robot.T_w_e}
+ )
# time.sleep(0.005)
- qs.append(new_q[:6])
- # plot this on the real robot
+ qs.append(sim_robot.q)
##############################################
# save the obtained joint-space trajectory #
@@ -62,7 +70,7 @@ def clikCartesianPathIntoJointPath(
# we're putting a dmp over this so we already have the timing ready
# TODO: make this general, you don't want to depend on other random
# arguments (make this one traj_time, then put tau0 = traj_time there
- t = np.linspace(0, args.tau0, len(qs)).reshape((len(qs), 1))
+ t = np.linspace(0, t_final, len(qs)).reshape((len(qs), 1))
joint_trajectory = np.hstack((t, qs))
# TODO handle saving more consistently/intentionally
# (although this definitely works right now and isn't bad, just mid)
@@ -70,5 +78,7 @@ def clikCartesianPathIntoJointPath(
# TODO: check if we actually need this and if not remove the saving
# whatever code uses this is responsible to log it if it wants it,
# let's not have random files around.
- np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=",", fmt="%.5f")
+ np.savetxt(
+ "./parameters/joint_trajectory.csv", joint_trajectory, delimiter=",", fmt="%.5f"
+ )
return joint_trajectory
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index c42b1420c140efb9581c6add0ccade6572335cca..dea74f558e82f7d3ae62cfa1d095dba5f38eab92 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -319,7 +319,7 @@ class AbstractRobotManager(abc.ABC):
NOTE: this function does not change internal variables!
because it shouldn't - it should only update the visualizer
"""
- if self.args.visualize_manipulator:
+ if self.args.visualizer:
self.visualizer_manager.sendCommand(viz_dict)
else:
if self.args.debug_prints:
diff --git a/python/smc/util/calib_board_hacks.py b/python/smc/util/calib_board_hacks.py
index e214412b5b9d3c51105fbb702585d37538ef70dd..a8e2bde5b014f734a7d845612d5cdc1976f56ff1 100644
--- a/python/smc/util/calib_board_hacks.py
+++ b/python/smc/util/calib_board_hacks.py
@@ -1,5 +1,10 @@
-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.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
@@ -21,28 +26,43 @@ 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: 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)
- parser.add_argument('--board-height', type=float, \
- help="height of the board (in meters) the robot will write on", \
- default=0.30)
- parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
- help="whether you want to do calibration", default=False)
- parser.add_argument('--n-calibration-tests', type=int, \
- help="number of calibration tests you want to run", default=10)
+
+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,
+ )
+ parser.add_argument(
+ "--board-height",
+ type=float,
+ help="height of the board (in meters) the robot will write on",
+ default=0.30,
+ )
+ parser.add_argument(
+ "--calibrate-plane",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to do calibration",
+ default=False,
+ )
+ parser.add_argument(
+ "--n-calibration-tests",
+ type=int,
+ help="number of calibration tests you want to run",
+ default=10,
+ )
return parser
-def fitNormalVector(positions:list[np.ndarray]) -> np.ndarray:
+def fitNormalVector(positions: list[np.ndarray]) -> np.ndarray:
"""
fitNormalVector
----------------
classic least squares fit.
there's also weighting to make new measurements more important,
- beucase we change the orientation of the end-effector as we go.
- the change in orientation is done so that the end-effector hits the
+ beucase we change the orientation of the end-effector as we go.
+ the change in orientation is done so that the end-effector hits the
board at the angle of the board, and thus have consistent measurements.
"""
positions = np.array(positions)
@@ -55,7 +75,12 @@ def fitNormalVector(positions:list[np.ndarray]) -> np.ndarray:
try:
# strong
W = np.diag(np.arange(1, len(positions) + 1))
- n_linearly_weighted = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
+ n_linearly_weighted = (
+ np.linalg.inv(positions.T @ W @ positions)
+ @ positions.T
+ @ W
+ @ np.ones(len(positions))
+ )
n_linearly_weighted = n_linearly_weighted / np.linalg.norm(n_linearly_weighted)
print("n_linearly_weighed", n_linearly_weighted)
print("if the following give you roughly the same number, it worked")
@@ -66,7 +91,10 @@ def fitNormalVector(positions:list[np.ndarray]) -> np.ndarray:
print("n_linearly_weighted is singular bruh")
return n_non_weighted
-def constructFrameFromNormalVector(R_initial_estimate:np.ndarray, n:np.ndarray) -> np.ndarray:
+
+def constructFrameFromNormalVector(
+ R_initial_estimate: np.ndarray, n: np.ndarray
+) -> np.ndarray:
"""
constructFrameFromNormalVector
----------------------------------
@@ -80,19 +108,22 @@ def constructFrameFromNormalVector(R_initial_estimate:np.ndarray, n:np.ndarray)
x_new = np.array([1.0, 0.0, 0.0])
y_new = np.cross(x_new, z_new)
# reshaping so that hstack works as expected
- R = np.hstack((x_new.reshape((3,1)), y_new.reshape((3,1))))
- R = np.hstack((R, z_new.reshape((3,1))))
+ R = np.hstack((x_new.reshape((3, 1)), y_new.reshape((3, 1))))
+ R = np.hstack((R, z_new.reshape((3, 1))))
# now ensure all the signs are the signs that you want,
# which we get from the initial estimate (which can not be that off)
# NOTE this is potentially just an artifact of the previous solution which relied
# on UR TCP readings which used rpy angles. but it ain't hurting nobody
# so i'm leaving it.
R = np.abs(R) * np.sign(R_initial_estimate)
- print('rot mat to new frame:')
- print(*R, sep=',\n')
+ print("rot mat to new frame:")
+ print(*R, sep=",\n")
return R
-def handleUserToHandleTCPPose(args:argparse.Namespace, robot:ForceTorqueOnSingleArmWrist)-> None:
+
+def handleUserToHandleTCPPose(
+ args: argparse.Namespace, robot: ForceTorqueOnSingleArmWrist
+) -> None:
"""
handleUserToHandleTCPPose
-----------------------------
@@ -103,7 +134,8 @@ def handleUserToHandleTCPPose(args:argparse.Namespace, robot:ForceTorqueOnSingle
4. release the freedrive and then start doing the calibration process
5. MAKE SURE THE END-EFFECTOR FRAME IS ALIGNED BY LOOKING AT THE MANIPULATOR VISUALIZER
"""
- print("""
+ print(
+ """
Whatever code you ran wants you to calibrate the plane on which you will be doing
your things. Put the end-effector at the top left corner SOMEWHAT ABOVE of the plane
where you'll be doing said things. \n
@@ -119,50 +151,72 @@ def handleUserToHandleTCPPose(args:argparse.Namespace, robot:ForceTorqueOnSingle
The robot will now enter freedrive mode so that you can manually put
the end-effector where it's supposed to be.\n
When you did it, press 'Y', or press 'n' to exit.
- """)
+ """
+ )
while True:
answer = input("Ready to calibrate or no (no means exit program)? [Y/n]")
- if answer == 'n' or answer == 'N':
- print("""
+ if answer == "n" or answer == "N":
+ print(
+ """
The whole program will exit. Change the argument to --no-calibrate or
change code that lead you here.
- """)
+ """
+ )
exit()
- elif answer == 'y' or answer == 'Y':
- print("""
+ elif answer == "y" or answer == "Y":
+ print(
+ """
The robot will now enter freedrive mode. Put the end-effector to the
top left corner of your plane and mind the orientation.
- """)
+ """
+ )
break
else:
- print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
+ print(
+ "Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!"
+ )
continue
- print("""
+ print(
+ """
Entering freedrive.
Put the end-effector to the top left corner of your plane and mind the orientation.
Press Enter to stop freedrive.
- """)
+ """
+ )
time.sleep(2)
- freedriveUntilKeyboard(args, robot)
+ freedriveUntilKeyboard(args, robot)
while True:
- answer = input("""
+ answer = input(
+ """
I am assuming you got the end-effector in the correct pose. \n
Are you ready to start calibrating or not (no means exit)? [Y/n]
- """)
- if answer == 'n' or answer == 'N':
+ """
+ )
+ if answer == "n" or answer == "N":
print("The whole program will exit. Goodbye!")
exit()
- elif answer == 'y' or answer == 'Y':
- print("Calibration about to start. Have your hand on the big red stop button!")
+ elif answer == "y" or answer == "Y":
+ print(
+ "Calibration about to start. Have your hand on the big red stop button!"
+ )
time.sleep(2)
break
else:
- print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
+ print(
+ "Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!"
+ )
continue
-def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist, plane_width:float, plane_height:float, n_tests:int) -> pin.SE3, np.ndarray:
+
+def calibratePlane(
+ args: argparse.Namespace,
+ robot: ForceTorqueOnSingleArmWrist,
+ plane_width: float,
+ plane_height: float,
+ n_tests: int,
+) -> tuple[pin.SE3, np.ndarray]:
"""
calibratePlane
--------------
@@ -171,7 +225,7 @@ def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist,
we sam
"""
handleUserToHandleTCPPose(args, robot)
- if args.pinocchio_only:
+ if not args.real:
robot._step()
q_init = robot.q
T_w_e = robot.T_w_e
@@ -188,7 +242,7 @@ def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist,
# used to define going to above new sample point on the board
go_above_new_sample_transf = pin.SE3.Identity()
- # go in the end-effector's frame z direction
+ # go in the end-effector's frame z direction
# our goal is to align that with board z
speed = np.zeros(6)
speed[2] = 0.02
@@ -198,20 +252,23 @@ def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist,
print("========================================")
time.sleep(0.01)
print("iteration number:", i)
- #robot.rtde_control.moveUntilContact(speed)
+ # robot.rtde_control.moveUntilContact(speed)
moveUntilContact(args, robot, speed)
- # no step because this isn't wrapped by controlLoopManager
+ # no step because this isn't wrapped by controlLoopManager
robot._step()
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))
+ 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:
+ if i < n_tests - 1:
current_pose = robot.T_w_e
- # go back up
+ # go back up
new_pose = current_pose.act(go_away_from_plane_transf)
moveL(args, robot, new_pose)
@@ -221,7 +278,9 @@ def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist,
print("going to new pose for detection", new_pose)
new_pose = init_pose.copy()
go_above_new_sample_transf.translation[0] = np.random.random() * plane_width
- go_above_new_sample_transf.translation[1] = np.random.random() * plane_height
+ go_above_new_sample_transf.translation[1] = (
+ np.random.random() * plane_height
+ )
new_pose = new_pose.act(go_above_new_sample_transf)
moveL(args, robot, new_pose)
# fix orientation
@@ -250,9 +309,11 @@ def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist,
new_pose.rotation = R
print("going back to initial position with fitted R")
moveL(args, robot, new_pose)
-
- print("i'll estimate the translation vector to board beginning now \
- that we know we're going straight down")
+
+ print(
+ "i'll estimate the translation vector to board beginning now \
+ that we know we're going straight down"
+ )
speed = np.zeros(6)
speed[2] = 0.02
@@ -264,32 +325,32 @@ def calibratePlane(args:argparse.Namespace, robot : ForceTorqueOnSingleArmWrist,
translation = T_w_e.translation.copy()
print("got translation vector, it's:", translation)
-
moveL(args, robot, new_pose)
q = robot.q
init_q = copy.deepcopy(q)
print("went back up, saved this q as initial q")
-
+
# put the speed slider back to its previous value
-# robot.setSpeedSlider(old_speed_slider)
- print('also, the translation vector is:', translation)
- if not args.pinocchio_only:
- file_path = './plane_pose.pickle'
+ # robot.setSpeedSlider(old_speed_slider)
+ print("also, the translation vector is:", translation)
+ if args.real:
+ file_path = "./parameters/plane_pose.pickle"
else:
- file_path = './plane_pose_sim.pickle'
- log_file = open(file_path, 'wb')
+ file_path = "./parameters/plane_pose_sim.pickle"
+ log_file = open(file_path, "wb")
plane_pose = pin.SE3(R, translation)
- log_item = {'plane_top_left_pose': plane_pose, 'q_init': q_init.copy()}
+ log_item = {"plane_top_left_pose": plane_pose, "q_init": q_init.copy()}
pickle.dump(log_item, log_file)
log_file.close()
return plane_pose, q_init
+
# TODO: update for the current year
-#if __name__ == "__main__":
+# if __name__ == "__main__":
# robot = RobotManager()
# # TODO make this an argument
# n_tests = 10
-# # TODO:
+# # TODO:
# # - tell the user what to do with prints, namely where to put the end-effector
# # to both not break things and also actually succeed
# # - start freedrive