diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..dab6cd932f3d42ae427684d8449babf19a525544
Binary files /dev/null and b/python/examples/.drawing_from_input_drawing.py.swp differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index a25e7131a5c190773851ae3c427bd3ca16872e65..caa9671d44d0494af92debfb4d85b32cd586d742 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -137,6 +137,12 @@ def getArgs():
help="whether you want to do calibration", default=True)
parser.add_argument('--n-calibration-tests', type=int, \
help="number of calibration tests you want to run", default=10)
+ parser.add_argument('--clik-goal-error', type=float, \
+ help="the clik error you are happy with", default=1e-2)
+ parser.add_argument('--max-init-clik-iterations', type=int, \
+ help="number of max clik iterations to get to the first point", default=10000)
+ parser.add_argument('--max-running-clik-iterations', type=int, \
+ help="number of max clik iterations between path points", default=1000)
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -226,7 +232,7 @@ if __name__ == "__main__":
# software setup #
#######################################################################
args = getArgs()
- clik_controller = getClikController(args)
+ clikController = getClikController(args)
robot = RobotManager(args)
#######################################################################
# drawing a path, making a joint trajectory for it #
@@ -251,7 +257,13 @@ if __name__ == "__main__":
raise NotImplementedError("you gotta give me that R somehow, 'cos there is no default atm")
# create a joint space trajectory based on the path
transf_body_to_board = pin.SE3(R, p)
- joint_trajectory = clikCartesianPathIntoJointPath(path, robot, transf_body_to_board)
+ #TODO find and fix q_init here
+ # ideally you obtain it as a part of the calibration process.
+ # of course you will or won't use/provide a new one based
+ # on some argument.
+ #q_init = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
+ joint_trajectory = clikCartesianPathIntoJointPath(path, args, robot, \
+ clikController, q_init, transf_body_task_frame)
# create DMP based on the trajectory
dmp = DMP(joint_trajectory)
diff --git a/python/ur_simple_control/clik/.clik_point_to_point.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..a49da600dc00a345bd81c6a1d2409732714750a6
Binary files /dev/null and b/python/ur_simple_control/clik/.clik_point_to_point.py.swp differ
diff --git a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..3cc7c72825cf0cd7f429c698dc4ced2dd40f36f5
Binary files /dev/null and b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp differ
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index f9469c1bb6524c33658bf814304f6977504aafbe..141b90d6ecb38c964b8f3e739911751062cb0bdd 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -41,14 +41,49 @@ def map2DPathTo3DPlane(path_2D, width, height):
return path
-# we now need a transformation from the body frame to the board
-# such that the first quadrant of the x-y plane is
-# in the bottom left corner of the board (obvious solution)
-def clikCartesianPathIntoJointPath(path, robot, transf_body_to_plane):
- transf_body_to_plane = pin.SE3(R, p)
+"""
+clikCartesianPathIntoJointPath
+------------------------------
+functionality
+------------
+Follows a provided Cartesian path,
+creates a joint space trajectory for it.
+
+return
+------
+- joint_space_trajectory to follow the given path.
+
+arguments
+----------
+- path:
+ --> cartesian path given in task frame
+TODO: write asserts for these arguments
+- args:
+ --> all the magic numbers used here better be here
+- clikController:
+ --> clik controller you're using
+- robot:
+ --> RobotManager instance (needed for forward kinematics etc)
+TODO: maybe it's better design to expect path in body frame idk
+ the good thing here is you're forced to think about frames.
+TODO: make this a kwarg with a neural transform as default.
+- transf_body_task_frame:
+ --> A transformation from the body frame to task frame.
+ --> It is assumed that the path was provided in the task frame
+ because 9/10 times that's more convenient,
+ and you can just pass a neural transform if you're not using it.
+- q_init:
+ --> starting point.
+ --> you can movej to it before executing the trajectory,
+ so this makes perfect sense to ensure correctness
+"""
+def clikCartesianPathIntoJointPath(path, args, robot, \
+ clikController, q_init, transf_body_task_frame):
+ transf_body_to_task_frame = pin.SE3(R, p)
for i in range(len(path)):
- path[i] = transf_body_to_plane.act(path[i])
+ path[i] = transf_body_to_task_frame.act(path[i])
+ # TODO remove print, write a test for this instead
print(path)
# TODO: finish this
@@ -58,45 +93,51 @@ def clikCartesianPathIntoJointPath(path, robot, transf_body_to_plane):
# skip inital pos tho
#q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
#q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
- q = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
- INIT_ITER = 10000
- n_iter = INIT_ITER
- RUNNING_ITER = 1000
+ # this is just init_q right now
+ # TODO: make this a flag or something for readability's sake
+ n_iter = args.max_init_clik_iterations
+ # 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,
+ # and we aren't pressed on time when turning it into an array later)
qs = []
for goal in path:
- # there's no orientation error (you do need it lmao)
- #Mgoal = pin.SE3(np.zeros((3,3)), goal)
Mgoal = pin.SE3(R, goal)
for i in range(n_iter):
- pin.forwardKinematics(model, data, q)
- SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
+ # TODO maybe hide pin call with RobotManager call
+ pin.forwardKinematics(robot.model, robot.data, q)
+ SEerror = robot.data.oMi[robot.JOINT_ID].actInv(Mgoal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
- if not n_iter == INIT_ITER:
+ if np.linalg.norm(err_vector) < args.clik_goal_error:
+ if not n_iter == args.args.max_init_clik_iterations:
print("converged in", i)
break
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
- v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
- #v = J.T @ err_vector
- q = pin.integrate(model, q, v * dt)
- if (not n_iter == INIT_ITER) and (i % 10 == 0):
+ J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ qd = clikController(J, err_vector)
+ # we're integrating this fully offline of course
+ q = pin.integrate(robot.model, q, qd * dt)
+ if (not n_iter == args.args.max_init_clik_iterations) and (i % 10 == 0):
qs.append(q[:6])
# just skipping the first run with one stone
- if n_iter == INIT_ITER:
- n_iter = RUNNING_ITER
+ if n_iter == args.args.max_init_clik_iterations:
+ n_iter = args.max_running_clik_iterations
else:
- if i == RUNNING_ITER-1:
+ if i == args.max_running_clik_iterations - 1:
print("DID NOT CONVERGE")
- #######################################################################
- # STEP 3: save joint path #
- #######################################################################
+ ##############################################
+ # save the obtained joint-space trajectory #
+ ##############################################
qs = np.array(qs)
- # let's assume it takes 10 seconds
- t = np.linspace(0, 10, len(qs)).reshape((len(qs),1))
- timed_qs = np.hstack((t, qs))
- np.savetxt("../new_traj.csv", timed_qs, delimiter=',', fmt='%.5f')
+ # 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))
+ joint_trajectory = np.hstack((t, qs))
+ # TODO handle saving more consistently/intentionally
+ # (although this definitely works right now and isn't bad, just mid)
+ np.savetxt("./new_traj.csv", joint_trajectory, delimiter=',', fmt='%.5f')
+ return joint_trajectory