diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 34d5150b00fca52523513be3e60db035c90685ef..2bf8e0f4efc9e18467a79152882be5b973c28fbd 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -195,13 +195,13 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
above_starting_write_point.translation[2] = -0.2
transf_bord_to_board = pin.SE3(rotation_matrix, translation_vector)
above_starting_write_point = transf_bord_to_board.act(above_starting_write_point)
- print("above_starting_write_point", above_starting_write_point)
- print("Mtool", robot.getMtool())
- Tinit = robot.getMtool().copy()
+ compliantMoveL(args, robot, above_starting_write_point)
+ Tinit = robot.getT_w_e().copy()
q_init = robot.getQ()
#exit()
#moveJ(args, robot, q_init)
compliantMoveL(args, robot, above_starting_write_point)
+ print("above_starting_write_point", above_starting_write_point)
# goal position, predefined
Tgoal = pin.SE3()
Tgoal.rotation = np.array([
@@ -215,14 +215,6 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
TgoalUP.translation += np.array([0,0,0.2])
#TgoalUP.translation += np.array([0,0,0.3])
- # instantiate controller
- #clik_controller = getClikController(args)
- #controlLoop = partial(controlLoopClik, robot, clik_controller)
- #log_item = {
- # 'qs' : np.zeros(robot.model.nq),
- # 'dqs' : np.zeros(robot.model.nq),
- # }
- #loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
print("going to above marker")
robot.Mgoal = TgoalUP.copy()
@@ -235,7 +227,7 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
#moveL(args, robot, Tgoal.copy())
#log_dict, final_iteration = loop_manager.run()
print("picking up marker")
- robot.gripper.move(255,255,255)
+ robot.closeGripper()
time.sleep(2)
print("going up")
robot.Mgoal = TgoalUP.copy()
@@ -251,11 +243,6 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
#moveL(args, robot, Tinit.copy())
#log_dict, final_iteration = loop_manager.run()
print("got back")
-# robot.stopHandler(None, None)
-# robot.stopHandler(None, None)
-# robot.stopHandler(None, None)
-# print("bye")
-# exit()
"""
findMarkerOffset
@@ -279,11 +266,10 @@ def findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init):
transf_bord_to_board = pin.SE3(rotation_matrix, translation_vector)
above_starting_write_point = transf_bord_to_board.act(above_starting_write_point)
print("above_starting_write_point", above_starting_write_point)
- print("Mtool", robot.getMtool())
+ print("current T_w_e", robot.getT_w_e())
#exit()
#moveJ(args, robot, q_init)
compliantMoveL(args, robot, above_starting_write_point)
- #Tinit = robot.getMtool().copy()
if args.board_axis == 'z':
axis_of_rot = rotation_matrix[:,2]
elif args.board_axis == 'y':
@@ -307,7 +293,7 @@ def findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init):
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.getMtool().translation
+ 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
#print("translation_vector", translation_vector)
@@ -361,7 +347,7 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
dmp.set_tau(tau_dmp)
q = robot.getQ()
- Mtool = robot.getMtool()
+ T_w_e = robot.getT_w_e()
# if args.board_axis == 'z':
# Z = np.diag(np.array([0.0, 0.0, 2.0, 1.0, 1.0, 1.0]))
# if args.board_axis == 'y':
@@ -479,12 +465,12 @@ if __name__ == "__main__":
q_init = np.array([ 1.32022870e+00, -1.40114772e+00, -1.27237797e+00, -1.15715911e+00,
1.76543856e+00, -2.38652054e-01, 2.94117647e-04, 2.94117647e-04])
#q_init = robot.getQ()
- #Mtool = robot.getMtool()
+ #T_w_e = robot.getT_w_e()
#rotation_matrix = np.array([
# [-1, 0, 0],
# [0, 0, -1],
# [0, -1, 0]])
- #translation_vector = Mtool.translation.copy()
+ #translation_vector = T_w_e.translation.copy()
# make the path 3D
path = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
@@ -570,8 +556,8 @@ if __name__ == "__main__":
#######################################################################
# TODO: add marker picking
# get up from the board
- robot._getMtool()
- current_pose = robot.getMtool()
+ robot._getT_w_e()
+ current_pose = robot.getT_w_e()
# Z
#current_pose.translation[2] = current_pose.translation[2] + 0.03
# Y
@@ -585,12 +571,12 @@ if __name__ == "__main__":
first_q.append(0.0)
first_q = np.array(first_q)
#pin.forwardKinematics(robot.model, robot.data, first_q)
- mtool = robot.getMtool(q_given=first_q)
+ mtool = robot.getT_w_e(q_given=first_q)
#mtool.translation[1] = mtool.translation[1] - 0.0035
#mtool.translation[1] = mtool.translation[1] - 0.012
mtool.translation[1] = mtool.translation[1] - 0.006
if args.debug_prints:
- print("im at:", robot.getMtool())
+ print("im at:", robot.getT_w_e())
print("going to:", mtool)
print('going to starting write position')
# TODO: write a compliantMoveL - be careful that everything is in the body frame
@@ -607,7 +593,7 @@ if __name__ == "__main__":
# and now we can actually run
log_dict, final_iteration = loop_manager.run()
#loop_manager.stopHandler(None, None)
- mtool = robot.getMtool()
+ mtool = robot.getT_w_e()
print("move a bit back")
if args.board_axis == 'z' or args.board_axis == 'y':
mtool.translation[1] = mtool.translation[1] - 0.1
@@ -615,6 +601,8 @@ if __name__ == "__main__":
# mtool.translation[2] = mtool.translation[2] - 0.1
compliantMoveL(args, robot, mtool)
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
#plotFromDict(log_dict, args)
loop_manager.stopHandler(None, None)
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 726a74d7c9f518f058d30432d300fc78c4aca580..d91143f67c24256355cba65f3f03936bdab840dd 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,213 +1,105 @@
-0.53241,0.58209
-0.53241,0.58209
-0.53039,0.58080
-0.52432,0.57951
-0.52432,0.57951
-0.52230,0.57821
-0.51825,0.57692
-0.50612,0.57046
-0.50005,0.56917
-0.49196,0.56400
-0.45758,0.55495
-0.44949,0.54978
-0.44342,0.54849
-0.43129,0.54591
-0.42522,0.54332
-0.42320,0.54332
-0.41309,0.54074
-0.41106,0.53945
-0.40904,0.53815
-0.39488,0.53557
-0.39286,0.53428
-0.39084,0.53428
-0.38882,0.53298
-0.38073,0.52782
-0.37870,0.52652
-0.37061,0.52135
-0.34634,0.50456
-0.33623,0.49939
-0.33016,0.49551
-0.30590,0.47871
-0.29578,0.47354
-0.28972,0.46966
-0.26342,0.45287
-0.25736,0.45157
-0.24927,0.44640
-0.21691,0.43348
-0.21286,0.43219
-0.21084,0.43090
-0.20680,0.42961
-0.20477,0.42831
-0.20477,0.42702
-0.20477,0.42702
-0.20882,0.42702
-0.21489,0.42702
-0.23309,0.42702
-0.24522,0.42702
-0.30185,0.42702
-0.32005,0.42961
-0.33421,0.42961
-0.37870,0.43477
-0.40095,0.43736
-0.41309,0.43736
-0.47174,0.44253
-0.48994,0.44253
-0.51219,0.44253
-0.56275,0.44770
-0.57690,0.44770
-0.58702,0.44770
-0.60926,0.44770
-0.65982,0.44770
-0.68207,0.44770
-0.68814,0.44770
-0.69016,0.44770
-0.69016,0.44770
-0.68612,0.44899
-0.68409,0.45028
-0.68207,0.45287
-0.66994,0.45545
-0.66185,0.46062
-0.65578,0.46191
-0.60724,0.47742
-0.59713,0.48000
-0.58702,0.48259
-0.54454,0.49163
-0.53039,0.49422
-0.47174,0.50456
-0.45758,0.50714
-0.44544,0.51102
-0.43331,0.51231
-0.39893,0.52135
-0.38882,0.52652
-0.38073,0.52911
-0.36859,0.53169
-0.36050,0.53298
-0.35443,0.53557
-0.34634,0.54074
-0.34432,0.54203
-0.34230,0.54332
-0.34028,0.54461
-0.34432,0.54332
-0.34432,0.54203
-0.34432,0.54203
-0.34432,0.54332
-0.34432,0.54332
-0.34432,0.54461
-0.34634,0.54849
-0.35241,0.56917
-0.35241,0.57821
-0.35646,0.58597
-0.37466,0.60406
-0.37870,0.61310
-0.38679,0.61827
-0.41309,0.63895
-0.42118,0.64412
-0.43533,0.65058
-0.47983,0.65962
-0.49398,0.66221
-0.50410,0.66479
-0.51825,0.66479
-0.54657,0.66867
-0.55263,0.66867
-0.55263,0.66867
-0.57488,0.66867
-0.57690,0.66738
-0.58499,0.66479
-0.59713,0.65962
-0.62342,0.64670
-0.62746,0.64282
-0.63353,0.63895
-0.65376,0.62215
-0.66185,0.61310
-0.66589,0.60923
-0.68612,0.58209
-0.69016,0.57434
-0.69825,0.56658
-0.71443,0.53686
-0.72252,0.53169
-0.72656,0.52265
-0.72656,0.51619
-0.74072,0.49034
-0.74072,0.48646
-0.74072,0.48259
-0.74072,0.46837
-0.74072,0.46579
-0.74072,0.46191
-0.74072,0.45416
-0.74072,0.44640
-0.73263,0.41798
-0.72859,0.41151
-0.71848,0.40118
-0.71645,0.39730
-0.69016,0.37662
-0.68207,0.37145
-0.66994,0.36887
-0.62949,0.34819
-0.62342,0.34690
-0.61129,0.34432
-0.56275,0.32881
-0.55061,0.32623
-0.54050,0.32623
-0.52634,0.32364
-0.47174,0.31847
-0.45151,0.31847
-0.43129,0.31460
-0.37466,0.31460
-0.35443,0.31460
-0.34230,0.31460
-0.30185,0.31847
-0.29578,0.32106
-0.28163,0.32364
-0.25736,0.33527
-0.24724,0.34044
-0.24118,0.34561
-0.21084,0.37016
-0.20275,0.37533
-0.19466,0.38308
-0.18657,0.38825
-0.14005,0.42961
-0.12388,0.43994
-0.10567,0.45545
-0.05713,0.48646
-0.04702,0.49680
-0.03691,0.50456
-0.01264,0.52006
-0.01466,0.52006
-0.01466,0.52135
-0.01669,0.52394
-0.01669,0.52523
-0.02478,0.53040
-0.05511,0.54203
-0.06320,0.54720
-0.13803,0.57175
-0.15421,0.57821
-0.16028,0.57951
-0.20680,0.58984
-0.22702,0.59243
-0.23915,0.59501
-0.24927,0.59501
-0.30387,0.60018
-0.31803,0.60277
-0.33016,0.60664
-0.38073,0.61052
-0.39488,0.61440
-0.40702,0.61440
-0.42118,0.61698
-0.43533,0.61698
-0.44140,0.61698
-0.44342,0.61698
-0.44949,0.61698
-0.45151,0.61698
-0.45353,0.61698
-0.46971,0.61052
-0.47174,0.60923
-0.47376,0.60535
-0.49601,0.58209
-0.49803,0.57821
-0.50410,0.56917
-0.51825,0.53945
-0.52230,0.53298
-0.52230,0.52911
-0.52432,0.52782
-0.52432,0.52782
-0.52432,0.52782
+0.44544,0.71519
+0.44544,0.71519
+0.44544,0.71390
+0.43938,0.71002
+0.43533,0.70614
+0.42724,0.70098
+0.41915,0.69581
+0.38882,0.67125
+0.37466,0.66350
+0.35848,0.65187
+0.34837,0.64282
+0.30792,0.60535
+0.29578,0.59114
+0.27556,0.57434
+0.24118,0.53557
+0.23106,0.52652
+0.22904,0.52265
+0.22500,0.51360
+0.22500,0.51231
+0.22500,0.51231
+0.22500,0.51102
+0.22702,0.50972
+0.23511,0.50456
+0.24320,0.49939
+0.28365,0.49034
+0.29376,0.48776
+0.30792,0.48517
+0.34432,0.48129
+0.36859,0.47742
+0.38882,0.47742
+0.44140,0.47742
+0.45353,0.48000
+0.48792,0.48905
+0.50814,0.49551
+0.66589,0.53428
+0.70836,0.54720
+0.76297,0.56271
+0.87016,0.59760
+0.89645,0.60923
+0.90050,0.61181
+0.90252,0.61310
+0.90252,0.61181
+0.90252,0.61310
+0.90252,0.61440
+0.89847,0.61440
+0.88836,0.61956
+0.88432,0.62086
+0.87016,0.62732
+0.86207,0.62990
+0.82364,0.64282
+0.80949,0.64541
+0.76701,0.65445
+0.74274,0.65833
+0.73061,0.66092
+0.67196,0.66609
+0.64971,0.66996
+0.63960,0.66996
+0.62949,0.67255
+0.60724,0.67513
+0.56275,0.68547
+0.54859,0.68547
+0.50410,0.69968
+0.49803,0.70098
+0.48387,0.70485
+0.48185,0.70485
+0.47174,0.70744
+0.46971,0.70744
+0.46769,0.70744
+0.46567,0.70744
+0.46365,0.70744
+0.45758,0.70744
+0.43938,0.70744
+0.38275,0.70098
+0.36859,0.70098
+0.35848,0.69968
+0.33623,0.69968
+0.33623,0.69839
+0.33421,0.69839
+0.33421,0.69710
+0.33421,0.69581
+0.33623,0.69193
+0.34028,0.68805
+0.35039,0.67772
+0.35241,0.67513
+0.36252,0.66479
+0.40095,0.61569
+0.41511,0.60406
+0.45556,0.54591
+0.46769,0.53169
+0.47780,0.51748
+0.51825,0.45803
+0.53039,0.44382
+0.54050,0.43477
+0.55263,0.41927
+0.56072,0.40764
+0.59713,0.36112
+0.60724,0.35207
+0.62949,0.32881
+0.63151,0.32493
+0.63151,0.32364
+0.63555,0.31976
+0.63555,0.31847
+0.63758,0.31847
+0.63758,0.31847
+0.63758,0.31847
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index d6e44d8e11465326921b4ad0e00348f84439a05b..bbcd15e78148acd5a796424e5d1111dd7a2a731e 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc
index 87d3c3d3d34dcdda2552ff64c9b31ada50dde781..8ae2b48b1fa2da019eb463d43cf61b3ecfed7067 100644
Binary files a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc and b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc
index 7e831d2ca1f5b2e9c6d95aab6f71489342542efd..4239e1bd51ab18db826e0888d6072ac43e223e4d 100644
Binary files a/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc and b/python/ur_simple_control/clik/__pycache__/clik_trajectory_following.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index 1bfaf269be6c532cf072520ae4eb41911243008c..9f43931a068b1ba10607587b589469652eaa3864 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -290,7 +290,7 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, q)
# this has forward kinematics in it
- Mtool = robot.getMtool()
+ T_w_e = robot.getT_w_e()
wrench = robot.getWrench()
# we need to overcome noise if we want to converge
if np.linalg.norm(wrench) < args.minimum_detectable_force_norm:
@@ -298,15 +298,15 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
save_past_dict['wrench'] = copy.deepcopy(wrench)
wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
mapping = np.zeros((6,6))
- mapping[0:3, 0:3] = Mtool.rotation
- mapping[3:6, 3:6] = Mtool.rotation
+ mapping[0:3, 0:3] = T_w_e.rotation
+ mapping[3:6, 3:6] = T_w_e.rotation
wrench = mapping.T @ wrench
#Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
wrench = Z @ wrench
#pin.forwardKinematics(robot.model, robot.data, q)
# first check whether we're at the goal
- SEerror = Mtool.actInv(robot.Mgoal)
+ SEerror = T_w_e.actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
# print("Convergence achieved, reached destionation!")
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 8baa74ed2477853ac7066204be53246e456bef88..bdd49bb1703b2195177a47ef3bc27a6fec1175f0 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -434,6 +434,9 @@ class RobotManager:
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
+ # do it once to get T_w_e
+ self._step()
+
#######################################################################
# getters which assume you called step() #
#######################################################################
@@ -444,8 +447,19 @@ class RobotManager:
def getQd(self):
return self.v_q.copy()
- def getMtool(self):
- return self.T_w_e.copy()
+ def getT_w_e(self, q_given=None):
+ if q_given is None:
+ return self.T_w_e.copy()
+ else:
+ assert type(q_given) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(self.model, self.data, q_given,
+ np.zeros(self.model.nv), np.zeros(self.model.nv))
+ return self.data.oMi[self.JOINT_ID].copy()
+
# this is in EE frame by default (handled in step which
# is assumed to be called before this)
@@ -574,9 +588,9 @@ class RobotManager:
self.q = q
"""
- _getMtool
+ _getT_w_e
-----
- NOTE: private function, use the getMtool() getter
+ NOTE: private function, use the getT_w_e() getter
urdf treats gripper as two prismatic joints,
but they do not affect the overall movement
of the robot, so we add or remove 2 items to the joint list.
@@ -585,7 +599,7 @@ class RobotManager:
NOTE: don't use this if use called _step() because it repeats forwardKinematics
"""
# TODO remove evil hack
- def _getMtool(self, q_given=None):
+ def _getT_w_e(self, q_given=None):
test = True
try:
test = q_given.all() == None
@@ -684,7 +698,7 @@ class RobotManager:
# TODO: do something better here (at least a better distribution)
self.wrench = np.random.random(self.n_arm_joints)
if not step_called:
- self._getMtool()
+ self._getT_w_e()
# NOTE: this mapping is equivalent to having a purely rotational action
# this is more transparent tho
mapping = np.zeros((6,6))
@@ -721,6 +735,21 @@ class RobotManager:
qd = np.array(qd)
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ def openGripper(self):
+ if (not self.args.simulation) and (not self.args.pinocchio_only):
+ self.gripper.move(0,255,255)
+ else:
+ print("not implemented yet, so nothing is going to happen!")
+
+ def closeGripper(self):
+ if (not self.args.simulation) and (not self.args.pinocchio_only):
+ self.gripper.move(255,255,255)
+ else:
+ print("not implemented yet, so nothing is going to happen!")
+
+#######################################################################
+# utility functions #
+#######################################################################
"""
defineGoalPointCLI
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index e68a522252105a4bc3597afb84715b32d856f166..82688cdec6204a4d6ea856a59f397797cb95e1e8 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ