diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index e8f0cf9924bbd4c123fcdc9cf58e2b3c93cdaf56..c40c23c490c43a9b9001a93bc44c2fce0a62027b 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/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 82aa642f4eee74e77dad389ccccdaa2651892031..45e09e3e1cb3ddfb458602f0e07a7f0aa3eee846 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 47977c2bfe829a356d1256c1170df6910239d235..8fd4b848a21b2b630645a4ddcc4eb220b5cbc727 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -63,6 +63,15 @@ def moveJP(args, robot, q_desired):
print("MoveJP done: convergence achieved, reached destionation!")
+# NOTE: it's probably a good idea to generalize this for different references:
+# - only qs
+# - qs and vs
+# - whatever special case
+# it could be better to have separate functions, whatever makes the code as readable
+# as possible.
+# also NOTE: there is an argument to be made for pre-interpolating the reference.
+# this is because joint positions will be more accurate.
+# if we integrate them with interpolated velocities.
def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
breakFlag = False
save_past_dict = {}
@@ -74,29 +83,50 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
t = i * robot.dt
# take the future (next) one
# TODO: turn this into interpolation
- t_index = int(np.ceil(t / reference['dt']))
+ t_index_lower = int(np.floor(t / reference['dt']))
+ t_index_upper = int(np.ceil(t / reference['dt']))
+
# TODO: move to a different function later
- if t_index >= len(reference['qs']):
- t_index = len(reference['qs']) - 1
- if stop_at_final:
- reference['vs'][t_index] = np.zeros(len(reference['vs'][t_index]))
+ if t_index_upper >= len(reference['qs']) - 1:
+ t_index_upper = len(reference['qs']) - 1
+ q_ref = reference['qs'][t_index_upper]
+ if (t_index_upper == len(reference['qs']) - 1) and stop_at_final:
+ v_ref = np.zeros(len(reference['vs'][t_index_upper]))
+ else:
+ v_ref = reference['vs'][t_index_upper]
# TODO: check if that's really the last
- #if t_index == len(reference['qs']) - 1:
+ #if t_index_upper == len(reference['qs']) - 1:
# breakFlag = True
- # TODO NOTE TODO TODO: remove/change
- if (t_index == len(reference['qs']) - 1) and \
- (np.linalg.norm(q - reference['qs'][-1]) < 1e-2):
+ # TODO NOTE TODO TODO: move under stop/don't stop at final argument
+ if (t_index_upper == len(reference['qs']) - 1) and \
+ (np.linalg.norm(q - reference['qs'][-1]) < 1e-2) and \
+ (np.linalg.norm(v) < 1e-2):
breakFlag = True
- error_q = reference['qs'][t_index] - q
- error_v = reference['vs'][t_index] - v
+ # TODO: set q_refs and v_refs once
+ if (t_index_upper < len(reference['qs']) - 1) and (not breakFlag):
+ #angle = (reference['qs'][t_index_upper] - reference['qs'][t_index_lower]) / reference['dt']
+ angle_v = (reference['vs'][t_index_upper] - reference['vs'][t_index_lower]) / reference['dt']
+ time_difference =t - t_index_lower * reference['dt']
+ v_ref = reference['vs'][t_index_lower] + angle_v * time_difference
+ # using pin.integrate to make this work for all joint types
+ # NOTE: not fully accurate as it could have been integrated with previous interpolated velocities,
+ # but let's go for it as-is for now
+ # TODO: make that work via past_data IF this is still too saw-looking
+ q_ref = pin.integrate(robot.model, reference['qs'][t_index_lower], reference['vs'][t_index_lower] * time_difference)
+
+
+
+
+ error_q = q_ref - q
+ error_v = v_ref - v
Kp = 1.0
- Kd = 0.1
+ Kd = 0.5
# feedforward feedback
- v_cmd = reference['vs'][t_index] + Kp * error_q + Kd * error_v
+ v_cmd = v_ref + Kp * error_q + Kd * error_v
qd_cmd = v_cmd[:6]
robot.sendQd(v_cmd)
@@ -104,8 +134,8 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
log_item['error_v'] = error_v
log_item['q'] = q
log_item['v'] = v
- log_item['reference_q'] = reference['qs'][t_index]
- log_item['reference_v'] = reference['vs'][t_index]
+ log_item['reference_q'] = q_ref
+ log_item['reference_v'] = v_ref
return breakFlag, {}, log_item
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index a422e1667bd1bebe55cce2ed816a8542a106ddca..3decba093e80e1377fe12691c3dd30f7a974897a 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -183,7 +183,7 @@ def controlLoopClik(robot, clik_controller, i, past_data):
return breakFlag, {}, log_item
-def moveUntilContactControlLoop(contact_detecting_force, speed, robot, clik_controller, i, past_data):
+def moveUntilContactControlLoop(contact_detecting_force, speed, robot : RobotManager, clik_controller, i, past_data):
"""
moveUntilContactControlLoop
---------------
@@ -198,7 +198,8 @@ def moveUntilContactControlLoop(contact_detecting_force, speed, robot, clik_cont
#wrench = robot.getWrench()
# you're already giving the speed in the EE i.e. body frame
# so it only makes sense to have the wrench in the same frame
- wrench = robot.getWrenchInEE()
+ #wrench = robot._getWrenchInEE()
+ wrench = robot.getWrench()
# and furthermore it's a reasonable assumption that you'll hit the thing
# in the direction you're going in.
# thus we only care about wrenches in those direction coordinates
@@ -241,7 +242,7 @@ def moveL(args, robot, goal_point):
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(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
@@ -383,6 +384,7 @@ def clikCartesianPathIntoJointPath(path, args, robot, \
transf_body_to_task_frame = pin.SE3(R, p)
q = copy.deepcopy(q_init)
+ pin.forwardKinematics(robot.model, robot.data, q)
for i in range(len(path)):
path[i] = transf_body_to_task_frame.act(path[i])
@@ -405,6 +407,8 @@ def clikCartesianPathIntoJointPath(path, args, robot, \
# and we aren't pressed on time when turning it into an array later)
qs = []
for goal in path:
+ #print("goal", goal)
+ #print("T_w_e", robot.data.oMi[robot.JOINT_ID])
Mgoal = pin.SE3(R, goal)
for i in range(n_iter):
# TODO maybe hide pin call with RobotManager call
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
index 01c73c9ba60d4bd126027f16c31dbe867c811c25..55668f2b64c34f7d4c980310fea151591c9ef526 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc and b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc differ
diff --git a/python/ur_simple_control/dmp/dmp.py b/python/ur_simple_control/dmp/dmp.py
index 348cb0ac12eebefc45154825a423bf25451af4c3..5c3c490cd19f387f09bbc580609642136e98171d 100644
--- a/python/ur_simple_control/dmp/dmp.py
+++ b/python/ur_simple_control/dmp/dmp.py
@@ -20,9 +20,6 @@ def getDMPArgs(parser):
#############################
parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
help="whether you want to use temporal coupling", default=True)
- parser.add_argument('--kp', type=float, \
- help="proportial control constant for position errors", \
- default=1.0)
parser.add_argument('--tau0', type=float, \
help="total time needed for trajectory. if you use temporal coupling,\
you can still follow the path even if it's too fast", \
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 3e4cbf9b0c26d7698bf4e363376a2526967d6205..91b4b43ad7ccb364c844688fd7b511a2024d08d4 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -125,7 +125,8 @@ def getMinimalArgParser():
# environment interaction parameters #
########################################
parser.add_argument('--contact-detecting-force', type=float, \
- default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+ #default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+ default=2.8, help='the force used to detect contact (collision) in the moveUntilContact function')
parser.add_argument('--minimum-detectable-force-norm', type=float, \
help="we need to disregard noise to converge despite filtering. \
a quick fix is to zero all forces of norm below this argument threshold.",
@@ -701,7 +702,7 @@ class RobotManager:
if test:
if not self.pinocchio_only:
q = self.rtde_receive.getActualQ()
- if self.args.gripper:
+ if self.args.gripper == "robotiq":
# TODO: make it work or remove it
#self.gripper_past_pos = self.gripper_pos
# this is pointless by itself
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/optimal_control.py
index e3fb969279a98988ac8ad84fff12e0db0b278096..4da6ba3fd504a8ce2f896bfcfef30e6fb798db3f 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/optimal_control.py
@@ -14,7 +14,7 @@ def get_OCP_args(parser : argparse.ArgumentParser):
help="time length between state-control-pair decision variables")
parser.add_argument("--n-knots", type=int, default=100, \
help="number of state-control-pair decision variables")
- parser.add_argument("--stop-at-final", type=int, default=True, \
+ parser.add_argument("--stop-at-final", type=int, default=1, \
help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
return parser
@@ -125,7 +125,13 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# NOTE: there are some possible parameters here that I'm not using
xs = [x0] * (solver.problem.T + 1)
us = solver.problem.quasiStatic([x0] * solver.problem.T)
- solver.solve(xs, us, 5000, False, 1e-9)
+
+ print("set goal:", goal)
+ start = time.time()
+ solver.solve(xs, us, 500, False, 1e-9)
+ end = time.time()
+ print("solved in:", end - start, "seconds")
+
#solver.solve()
# get reference (state trajectory)
# we aren't using controls because we only have velocity inputs
@@ -143,10 +149,10 @@ if __name__ == "__main__":
robot = RobotManager(args)
# TODO: remove once things work
robot.max_qd = 3.14
+ print("velocity limits", robot.model.velocityLimit)
robot.q = pin.randomConfiguration(robot.model)
goal = pin.SE3.Random()
goal.translation = np.random.random(3) * 0.4
- print("set goal:", goal)
reference, solver = IKOCP(args, robot, goal)
# we only work within -pi - pi (or 0-2pi idk anymore)
#reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc
index e346abfc3404c5e04c001cf4c8c41732952a3921..9550e0a266df8ffbbcd7f29cd2dc60a649e809dd 100644
Binary files a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index d53d64851338f3a49892fe0f8a7fe3b2ce11ef08..1d7887c99474bd17a18b30043a0628b05053a458 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -16,11 +16,13 @@ def getBoardCalibrationArgs(parser):
parser.add_argument('--board-width', type=float, \
help="width of the board (in meters) the robot will write on", \
#default=0.5)
- default=0.25)
+ #default=0.25)
+ default=0.20)
parser.add_argument('--board-height', type=float, \
help="height of the board (in meters) the robot will write on", \
#default=0.35)
- default=0.25)
+ #default=0.25)
+ default=0.20)
# TODO: add axis direction too!!!!!!!!!!!!!!!!!
# and change the various offsets accordingly
parser.add_argument('--board-axis', type=str, \
@@ -303,6 +305,7 @@ def calibratePlane(args, robot, plane_width, plane_height, n_tests):
# THIS IS Y
if args.board_axis == 'y':
new_pose.translation[2] = init_pose.translation[2] - np.random.random() * plane_height
+ print("new pose for detection", new_pose)
moveL(args, robot, new_pose)
# fix orientation
new_pose.rotation = R