diff --git a/python/examples/clik.py b/python/examples/clik.py
index 898ee14caf6e19736b16bd10837e2d5833cfc7d0..e84b7ade552b2811dea3276bf9da3aa8570d409c 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -87,9 +87,10 @@ if __name__ == "__main__":
Mgoal = robot.defineGoalPointCLI()
compliantMoveL(args, robot, Mgoal)
print("stopping via freedrive lel")
- robot.rtde_control.freedriveMode()
- time.sleep(0.5)
- robot.rtde_control.endFreedriveMode()
+ if not args.pinocchio_only:
+ robot.rtde_control.freedriveMode()
+ time.sleep(0.5)
+ robot.rtde_control.endFreedriveMode()
#ControlLoopManager.stopHandler(None, None, None)
#clik_controller = getClikController(args)
#controlLoop = partial(controlLoopClik, robot, clik_controller)
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index b459e3391c8d3fd698b866eb207a20a300e90b97..0a171bd89b90f1534844b0e81bdd772433b0371f 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -3,10 +3,6 @@
# 2. delete the unnecessary comments
# 8. add some code to pick up the marker from a prespecified location
# 10. write documentation as you go along
-# BIG TODO:
-# DOES NOT WORK WITH SPEED SLIDER =/= 1.0!!!!!!!!!!!!!!!!!!!!!!!!!!!
-# realistic solution: keep it at 1.0 at all times. make acceleration and/or
-# speeds small
import pinocchio as pin
import numpy as np
@@ -363,11 +359,6 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
dmp.set_tau(tau_dmp)
q = robot.getQ()
Mtool = robot.getMtool()
- # TODO look into UR code/api for estimating the same
- # based on currents in the joints.
- # it's probably worse, but maybe some sensor fusion-type thing
- # is actually better, who knows.
- # also you probably want to do the fusion of that onto tau (got from J.T @ wrench)
# 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':
@@ -390,15 +381,8 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
# it's just coordinate change from base to end-effector,
# they're NOT the same rigid body,
# the vector itself is not changing, only the coordinate representation
- mapping = np.zeros((6,6))
- mapping[0:3, 0:3] = Mtool.rotation
- mapping[3:6, 3:6] = Mtool.rotation
- wrench = mapping.T @ wrench
wrench = Z @ wrench
-# if i % 100 == 0:
-# print(*wrench.round(1))
- pin.forwardKinematics(robot.model, robot.data, q)
J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
dq = robot.getQd()[:6].reshape((6,1))
# get joint
@@ -583,6 +567,7 @@ if __name__ == "__main__":
#######################################################################
# TODO: add marker picking
# get up from the board
+ robot._getMtool()
current_pose = robot.getMtool()
# Z
#current_pose.translation[2] = current_pose.translation[2] + 0.03
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index 9a68dc112406fb605a31fe76116d3e062f5e5c11..10d5de6f705de72ce14477d9e6e19bf159a6d519 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -163,7 +163,7 @@ def controller():
pass
# control loop to be passed to ControlLoopManager
-def controlLoopPointImpedance(q_init, controller, robot, i, past_data):
+def controlLoopPointImpedance(q_init, controller, robot : RobotManager, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -172,49 +172,17 @@ def controlLoopPointImpedance(q_init, controller, robot, i, past_data):
Mtool = robot.getMtool()
wrench = robot.getWrench()
log_item['wrench_raw'] = wrench.reshape((6,))
- save_past_dict['wrench'] = copy.deepcopy(wrench)
# deepcopy for good coding practise (and correctness here)
-# save_past_dict['wrench'] = copy.deepcopy(wrench)
+ save_past_dict['wrench'] = copy.deepcopy(wrench)
# 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]
- #Z = np.diag(np.array([0.6, 0.6, 1.0, 0.5, 0.5, 0.5]))
- #Z = np.diag(np.array([0.6, 1.0, 0.6, 0.5, 0.5, 0.5]))
- #Z = np.diag(np.array([1.0, 1.0, 0.1, 1.0, 1.0, 1.0]))
- #Z = np.diag(np.ones(6))
- #copy_wrench = copy.deepcopy(wrench)
-# wrench[1] =
Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
#Z = np.diag(np.ones(6))
- ##### TODO try this cartesian test
- #spatial_x = pin.SE3().setIdentity()
- #spatial_x.translation = np.array([1.0, 0.0, 0.0])
- #Z = Mtool.actInv(spatial_x).vector
- #Z = np.diag(np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
- #Z = np.diag(np.array([1.0, 0.6, 1.0, 0.5, 0.5, 0.5]))
- # the robot gives the wrench in the base frame for some unknown reason
- #wrench = robot.getMtool().toDualActionMatrix().T @ wrench
- mapping = np.zeros((6,6))
- mapping[0:3, 0:3] = Mtool.rotation
- mapping[3:6, 3:6] = Mtool.rotation
- #TODO # this might be expressed in the spatial frame
- wrench = mapping.T @ wrench
wrench = Z @ wrench
-# if i % 25 == 0:
-# #print(Z)
-# print("================================")
-# print(*wrench.round(1))
-# if i % 25 == 0:
-# for w in wrench:
-# if w < 0:
-# print(np.round(w, 3), end=", ")
-# else:
-# print(np.round(w, 3), end=', ')
-# print("")
- pin.forwardKinematics(robot.model, robot.data, q)
# this jacobian starts at the end of the end-effector.
# but the wrench is probably modified wrt tip of the end effector
# because we defined the load in the ur inteface thing.
@@ -271,14 +239,8 @@ def controlLoopCartesianPointImpedance(Mtool_init, clik_controller, robot, i, pa
# but let's stick to the default for now
#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, 10.0, 10.0, 10.0]))
- mapping = np.zeros((6,6))
- mapping[0:3, 0:3] = Mtool.rotation
- mapping[3:6, 3:6] = Mtool.rotation
- #TODO # this might be expressed in the spatial frame
- wrench = mapping.T @ wrench
wrench = Z @ wrench
- pin.forwardKinematics(robot.model, robot.data, q)
# this jacobian starts at the end of the end-effector.
# but the wrench is probably modified wrt tip of the end effector
# because we defined the load in the ur inteface thing.
@@ -324,9 +286,6 @@ if __name__ == "__main__":
print("you will get a lot of stuff printed out, as requested")
clikController = getClikController(args)
robot = RobotManager(args)
-
- # calibrate FT first
- #wrench_offset = robot.calibrateFT()
# TODO and 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
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 2e899767fbf6f9cb906181df79c9c64f46d78ea2..0f8cb90a1daf3358e56973d9e779bc1f11d293db 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 f6eb30f9a3134ffe664f07421d12d9180a5ab800..6d36d3cc2869d69bfb4882945aab311e5bbb2d56 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/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index bf86a9a5e5d6b42a4452a3151f6baf8bf7a08798..fd2c10f77bf42a1a02052b4b53ffc2957fc4dea9 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -282,7 +282,7 @@ generic control loop for clik (handling error to final point etc).
in some version of the universe this could be extended to a generic
point-to-point motion control loop.
"""
-def controlLoopCompliantClik(args, robot, i, past_data):
+def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
breakFlag = False
log_item = {}
save_past_dict = {}
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 99cbf332429b9e41fec285280f0be461fad5a2a5..243d7b5a4a2457b54d12fd0b2a1805fbd519526f 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -41,19 +41,18 @@ The point of these managers is to handle:
for the real robot and various simulations with single commands
that just do exactly what you want them to do
+
current state
-------------
-Everything is UR specific.
-In the process of removing everything that isn't a speedJ command
-from the interface though.
-After that point, it will be immediatelly trasferable if you can
-give me a robot with a speedJ command
-and a correct urdf model of it (so that I can load it into pinocchio).
+Everything is UR specific or supports pinocchio only simulation,
+and only velocity-controlled robot functions exist.
long term vision
-------------------
Cut out the robot-specific parts out of the manager classes,
and create child classes for particular robots.
+There is an interface to a physics simulator.
+Functions for torque controlled robots exist.
"""
"""
@@ -156,6 +155,8 @@ class ControlLoopManager:
self.final_iteration = 0
for i in range(self.max_iterations):
start = time.time()
+ # NOTE: all required pre-computations are handled here
+ self.robot_manager._step()
# TODO make the arguments to controlLoop kwargs or whatever
# so that you don't have to declare them on client side if you're not using them
breakFlag, latest_to_save_dict, log_item = self.controlLoop(i, self.past_data)
@@ -247,7 +248,7 @@ class ControlLoopManager:
stop the robot.
NOTE: apparently this isn't enough,
nor does stopJ do anything, so it goes to freedriveMode
- and exists, which actually stops it.
+ and then exits it, which actually stops ur robots at least.
"""
def stopHandler(self, signum, frame):
print('sending 300 speedjs full of zeros and exiting')
@@ -321,7 +322,9 @@ robotmanager:
actually run at 500Hz and not more.
- this is probably not the most new-user friendly solution,
but it's designed for fastest idea to implementation rate.
-- if this was a real programming language, all of these would be private variables.
+- if this was a real programming language, all of these would really be private variables.
+ as it currently stands, "private" functions have the '_' prefix
+ while the public getters don't have a prefix.
- TODO: write out default arguments needed here as well
"""
class RobotManager:
@@ -430,6 +433,26 @@ class RobotManager:
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
+#######################################################################
+# getters which assume you called step() #
+#######################################################################
+
+ def getQ(self):
+ return self.q.copy()
+
+ def getQd(self):
+ return self.v_q.copy()
+
+ def getMtool(self):
+ return self.T_w_e.copy()
+
+ # this is in EE frame by default (handled in step which
+ # is assumed to be called before this)
+ def getWrench(self):
+ return self.wrench.copy()
+
+
+
"""
calibrateFT
-----------
@@ -445,6 +468,12 @@ class RobotManager:
print("Will read from f/t sensors for a some number of seconds")
print("and give you the average.")
print("Use this as offset.")
+ # NOTE: zeroFtSensor() needs to be called frequently because it drifts
+ # by quite a bit in a matter of minutes.
+ # if you are running something on the robot for a long period of time, you need
+ # to reapply zeroFtSensor() to get reasonable results.
+ # because the robot needs to stop for the zeroing to make sense,
+ # this is the responsibility of the user!!!
self.rtde_control.zeroFtSensor()
for i in range(2000):
start = time.time()
@@ -461,9 +490,8 @@ class RobotManager:
return self.wrench_offset.copy()
"""
- step
+ _step
----
- - NOTE and TODO: not used ATM
- the idea is to update everything that should be updated
on a step-by-step basis
- the actual problem this is solving is that you're not calling
@@ -482,17 +510,24 @@ class RobotManager:
also TODO: make ifs for the simulation etc.
this is less ifs overall right.
"""
- def step(self):
- self.getQ()
- self.getQd()
- self.getWrench()
- # certainly not necessary btw
+ def _step(self):
+ self._getQ()
+ self._getQd()
+ #self._getWrench()
+ # computeAllTerms is certainly not necessary btw
# but if it runs on time, does it matter? it makes everything available...
- pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
+ # (includes forward kinematics, all jacobians, all dynamics terms, energies)
+ # NOTE: it's too slow
+ #pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
+ pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+ # wrench in EE should obviously be the default
+ self._getWrenchInEE(step_called=True)
# this isn't real because we're on a velocity-controlled robot,
# so this is actually None (no tau, no a_q, as expected)
self.a_q = self.data.ddq
+ # TODO NOTE: you'll want to do the additional math for
+ # torque controlled robots here, but it's ok as is rn
"""
setSpeedSlider
@@ -506,15 +541,16 @@ class RobotManager:
self.speed_slider = value
"""
- getQ
+ _getQ
-----
+ NOTE: private function for use in _step(), use the getter getQ()
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.
also, the gripper is controlled separately so we'd need to do this somehow anyway
NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
"""
- def getQ(self):
+ def _getQ(self):
if not self.pinocchio_only:
q = self.rtde_receive.getActualQ()
if self.args.gripper:
@@ -535,21 +571,20 @@ class RobotManager:
# readability is a somewhat subjective quality after all
q = np.array(q)
self.q = q
- else:
- return self.q
- return q
"""
- getMtool
+ _getMtool
-----
+ NOTE: private function, use the getMtool() 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.
also, the gripper is controlled separately so we'd need to do this somehow anyway
- NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO.
+ NOTE: don't use this if use called _step() because it repeats forwardKinematics
"""
# TODO remove evil hack
- def getMtool(self, q_given=None):
+ def _getMtool(self, q_given=None):
test = True
try:
test = q_given.all() == None
@@ -584,18 +619,18 @@ class RobotManager:
pin.forwardKinematics(self.model, self.data, q)
# TODO probably remove deepcopy
self.T_w_e = self.data.oMi[self.JOINT_ID]
- return self.T_w_e.copy()
"""
- getQd
+ _getQd
-----
- same note as getQ.
+ NOTE: private function, use the _getQd() getter
+ same note as _getQ.
TODO NOTE: atm there's no way to get current gripper velocity.
this means you'll probably want to read current positions and then finite-difference
to get the velocity.
as it stands right now, we'll just pass zeros in because I don't need this ATM
"""
- def getQd(self):
+ def _getQd(self):
if not self.pinocchio_only:
qd = self.rtde_receive.getActualQd()
if self.args.gripper:
@@ -618,44 +653,44 @@ class RobotManager:
# readability is a somewhat subjective quality after all
qd = np.array(qd)
self.v_q = qd
- return self.v_q.copy()
"""
- getWrench
+ _getWrench
-----
different things need to be send depending on whether you're running a simulation,
you're on a real robot, you're running some new simulator bla bla. this is handled
here because this things depend on the arguments which are manager here (hence the
class name RobotManager)
"""
- def getWrenchRaw(self):
+ def _getWrenchRaw(self):
if not self.pinocchio_only:
wrench = np.array(self.rtde_receive.getActualTCPForce())
else:
raise NotImplementedError("Don't have time to implement this right now.")
- return wrench
- def getWrench(self):
+ def _getWrench(self):
if not self.pinocchio_only:
self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
else:
+ # TODO: do something better here (at least a better distribution)
self.wrench = np.random.random(self.n_arm_joints)
- return self.wrench.copy()
- def getWrenchInEE(self):
+ def _getWrenchInEE(self, step_called=False):
if not self.pinocchio_only:
self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
else:
+ # TODO: do something better here (at least a better distribution)
self.wrench = np.random.random(self.n_arm_joints)
- self.getMtool()
+ if not step_called:
+ self._getMtool()
# NOTE: this mapping is equivalent to having a purely rotational action
# this is more transparent tho
mapping = np.zeros((6,6))
mapping[0:3, 0:3] = self.T_w_e.rotation
mapping[3:6, 3:6] = self.T_w_e.rotation
self.wrench = mapping.T @ self.wrench
- return self.wrench.copy()
+
"""
sendQd
-----
@@ -689,6 +724,7 @@ class RobotManager:
"""
defineGoalPointCLI
------------------
+ NOTE: this assume _step has not been called because it's run before the controlLoop
--> best way to handle the goal is to tell the user where the gripper is
in both UR tcp frame and with pinocchio and have them
manually input it when running.
@@ -700,6 +736,7 @@ class RobotManager:
in the case you're visualizing, makes no sense otherwise.
"""
def defineGoalPointCLI(self):
+ self._getQ()
q = self.getQ()
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))