diff --git a/python/examples/camera_no_lag.py b/python/examples/camera_no_lag.py
index 41c1406e9e45e6353af6b99cbb2714cc04143bed..180c52e8a8b151d4e6b08870ace21b8c42655415 100644
--- a/python/examples/camera_no_lag.py
+++ b/python/examples/camera_no_lag.py
@@ -1,7 +1,8 @@
import cv2
-#camera = cv2.VideoCapture(0)
-camera = cv2.VideoCapture("http://192.168.219.153:8080/video")
+camera = cv2.VideoCapture(0)
+# if you have ip_webcam set-up you can do this (but you have put in the correct ip)
+#camera = cv2.VideoCapture("http://192.168.219.153:8080/video")
while True:
(grabbed, frame) = camera.read()
diff --git a/python/examples/clik.py b/python/examples/clik.py
index dd6791f7ec2e601110679a3063a064957da7c576..99867e0bf4a61c04763b211d86f5d006bd960c72 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -4,24 +4,7 @@ import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
-# TODO write this in managers and automate name generation
-
-"""
-Every imaginable magic number, flag and setting is put in here.
-This way the rest of the code is clean.
-If you want to know what these various arguments do, just grep
-though the code to find them (but replace '-' with '_' in multi-word arguments).
-All the sane defaults are here, and you can change any number of them as an argument
-when running your program. If you want to change a drastic amount of them, and
-not have to run a super long commands, just copy-paste this function and put your
-own parameters as default ones.
-NOTE1: the args object obtained from args = parser.get_args()
-is pushed all around the rest of the code (because it's tidy), so don't change their names.
-NOTE2: that you need to copy-paste and add aguments you need
-to the script you will be writing. This is kept here
-only as a convenient reference point.
-"""
def get_args():
parser = getMinimalArgParser()
parser.description = 'Run closed loop inverse kinematics \
@@ -33,17 +16,12 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- #Mgoal = robot.defineGoalPointCLI()
- #compliantMoveL(args, robot, Mgoal)
- #robot.closeGripper()
- time.sleep(6.0)
+ Mgoal = robot.defineGoalPointCLI()
+ compliantMoveL(args, robot, Mgoal)
robot.closeGripper()
- #robot.openGripper()
+ robot.openGripper()
if not args.pinocchio_only:
- print("stopping via freedrive lel")
- robot.rtde_control.freedriveMode()
- time.sleep(0.5)
- robot.rtde_control.endFreedriveMode()
+ robot.stopRobot()
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
diff --git a/python/examples/comparing_logs_example.py b/python/examples/comparing_logs_example.py
new file mode 100644
index 0000000000000000000000000000000000000000..c59097852b909e385717e42dec7a4779de0de1a0
--- /dev/null
+++ b/python/examples/comparing_logs_example.py
@@ -0,0 +1,27 @@
+from ur_simple_control.visualize.manipulator_comparison_visualizer import getLogComparisonArgs, ManipulatorComparisonManager
+import time
+
+args = getLogComparisonArgs()
+cmp_manager = ManipulatorComparisonManager(args)
+
+print("""
+this code animates 2 manipulator and can run an animated
+plot along side it with the same timing.
+you're supposed to select what you want to plot yourself.
+the comparison manager has no idea how many control loops
+you have nor what's logged in them, apart from the fact
+that everything has to have qs to visualize the manipulators.
+here we're assuming you have 1 control loop per log,
+and that the same things are logged.
+""")
+
+key = list(cmp_manager.logm1.loop_logs.keys())[0]
+cmp_manager.createRunningPlot(cmp_manager.logm1.loop_logs[key], 0, len(cmp_manager.logm1.loop_logs[key]['qs']))
+cmp_manager.createRunningPlot(cmp_manager.logm2.loop_logs[key], 0, len(cmp_manager.logm2.loop_logs[key]['qs']))
+cmp_manager.visualizeWholeRuns()
+cmp_manager.manipulator_visualizer_cmd_queue.put("befree")
+print("main done")
+time.sleep(0.1)
+cmp_manager.manipulator_visualizer_process.terminate()
+if args.debug_prints:
+ print("terminated manipulator_visualizer_process")
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
new file mode 100644
index 0000000000000000000000000000000000000000..b8e3940cbf38901fe5f7baa9633da7f8f4a26da7
--- /dev/null
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -0,0 +1,45 @@
+import numpy as np
+import time
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, CrocoIKOCP
+from ur_simple_control.basics.basics import jointTrajFollowingPD
+from ur_simple_control.util.logging_utils import LogManager
+import pinocchio as pin
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+ Mgoal = robot.defineGoalPointCLI()
+ # create and solve the optimal control problem of
+ # getting from current to goal end-effector position.
+ # reference is position and velocity reference (as a dictionary),
+ # while solver is a crocoddyl object containing a lot more information
+ reference, solver = CrocoIKOCP(args, robot, Mgoal)
+ # we need a way to follow the reference trajectory,
+ # both because there can be disturbances,
+ # and because it is sampled at a much lower frequency
+ jointTrajFollowingPD(args, robot, reference)
+
+ print("final position:")
+ print(robot.getT_w_e())
+
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ #loop_manager.stopHandler(None, None)
+
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index f602be7223fc3d95390bb3de8d029e3fe73cc0d4..fa8caa18c3b1e0fc08b4d435b656d67e4ad1113c 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -1,9 +1,3 @@
-#, this is the main file for the drawing your drawings with a dmp with force feedback
-# TODO:
-# 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
-
import pinocchio as pin
import numpy as np
import matplotlib.pyplot as plt
@@ -65,6 +59,7 @@ def getArgs():
# go and pick up the marker
# marker position:
+# NOTE: this function does not really work (needs more points)
"""
R =
-0.73032 -0.682357 0.0319574
@@ -283,9 +278,9 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
# log what you said you'd log
# TODO fix the q6 situation (hide this)
log_item['qs'] = q[:6].reshape((6,))
- log_item['dmp_poss'] = dmp.pos.reshape((6,))
- log_item['dqs'] = dq.reshape((6,))
- log_item['dmp_vels'] = dmp.vel.reshape((6,))
+ log_item['dmp_qs'] = dmp.pos.reshape((6,))
+ log_item['vs'] = dq.reshape((6,))
+ log_item['dmp_vs'] = dmp.vel.reshape((6,))
log_item['wrench'] = wrench.reshape((6,))
log_item['tau'] = tau.reshape((6,))
@@ -332,7 +327,7 @@ if __name__ == "__main__":
print(translation_vector)
exit()
else:
- # TODO: save this somewhere obviously
+ # TODO: save this somewhere obviously (side file)
# also make it prettier if possible
print("using predefined values")
#q_init = np.array([1.4545, -1.7905, -1.1806, -1.0959, 1.6858, -0.1259, 0.0, 0.0])
diff --git a/python/examples/oc_for_ik.py b/python/examples/oc_for_ik.py
deleted file mode 100644
index da2a7e98ace1ece389969b960e333372ee10027a..0000000000000000000000000000000000000000
--- a/python/examples/oc_for_ik.py
+++ /dev/null
@@ -1,39 +0,0 @@
-import numpy as np
-import time
-import argparse
-from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-# TODO: implement
-from ur_simple_control.optimal_control.optimal_control import IKOCP
-# TODO: implement
-from ur_simple_control.basics.basics import jointTrajFollowingPID
-from ur_simple_control.util.logging_utils import saveLog
-import pinocchio as pin
-
-
-def get_args():
- parser = getMinimalArgParser()
-
-if __name__ == "__main__":
- args = get_args()
- robot = RobotManager(args)
- #Mgoal = robot.defineGoalPointCLI()
- Mgoal = pin.SE3.Identity()
- Mgoal.translation = np.array([0.3,0.3,0.3])
- traj = IKOCP(robot, Mgoal)
- exit()
-
- #log_dict, final_iteration = compliantMoveL(args, robot, Mgoal)
- if not args.pinocchio_only:
- print("stopping via freedrive lel")
- robot.rtde_control.freedriveMode()
- time.sleep(0.5)
- robot.rtde_control.endFreedriveMode()
-
- if args.visualize_manipulator:
- robot.killManipulatorVisualizer()
-
- if args.save_log:
- saveLog(log_dict, final_iteration, args)
- #loop_manager.stopHandler(None, None)
-
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index c40c23c490c43a9b9001a93bc44c2fce0a62027b..b59fcce0499fab94884093e38ecf948933a2717a 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 45e09e3e1cb3ddfb458602f0e07a7f0aa3eee846..e1ec0d0b99d4fb96948b9ab5c2c7409a97300de5 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 8fd4b848a21b2b630645a4ddcc4eb220b5cbc727..aeb95345ce0ab563d438da95c9d10e2bc1e0b472 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -126,16 +126,17 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
Kd = 0.5
# feedforward feedback
- v_cmd = v_ref + 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)
- log_item['error_q'] = error_q
- log_item['error_v'] = error_v
- log_item['q'] = q
- log_item['v'] = v
- log_item['reference_q'] = q_ref
- log_item['reference_v'] = v_ref
+ log_item['error_qs'] = error_q
+ log_item['error_vs'] = error_v
+ log_item['qs'] = q
+ log_item['vs'] = v
+ log_item['vs_cmd'] = v_cmd
+ log_item['reference_qs'] = q_ref
+ log_item['reference_vs'] = v_ref
return breakFlag, {}, log_item
@@ -143,12 +144,13 @@ def jointTrajFollowingPD(args, robot, reference):
# we're not using any past data or logging, hence the empty arguments
controlLoop = partial(jointTrajFollowingPDControlLoop, args.stop_at_final, robot, reference)
log_item = {
- 'error_q' : np.zeros(robot.model.nq),
- 'error_v' : np.zeros(robot.model.nv),
- 'q' : np.zeros(robot.model.nq),
- 'v' : np.zeros(robot.model.nv),
- 'reference_q' : np.zeros(robot.model.nq),
- 'reference_v' : np.zeros(robot.model.nv)
+ 'error_qs' : np.zeros(robot.model.nq),
+ 'error_vs' : np.zeros(robot.model.nv),
+ 'qs' : np.zeros(robot.model.nq),
+ 'vs' : np.zeros(robot.model.nv),
+ 'vs_cmd' : np.zeros(robot.model.nv),
+ 'reference_qs' : np.zeros(robot.model.nq),
+ 'reference_vs' : np.zeros(robot.model.nv)
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
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 55668f2b64c34f7d4c980310fea151591c9ef526..4e6126a9fbe330cb962c4cf2245ed8776e8eb631 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/managers.py b/python/ur_simple_control/managers.py
index eb33ff86b2a7a8a06a9ab71eff293aec8f4f337c..dfffff597f4062b3daf7d7f7ffaec21bea628a1c 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -100,9 +100,13 @@ def getMinimalArgParser():
very useful and convenient when running simulation before running on real", \
default=False)
parser.add_argument('--acceleration', type=float, \
- help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.3. \
BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
TODO: check what this means", default=0.3)
+ parser.add_argument('--max-qd', type=float, \
+ help="robot's joint velocities [rad/s]. scalar positive constant, max 3.14, and default 0.5. \
+ BE CAREFUL WITH THIS. also note that wrist joints can go to 6.28 rad, but here \
+ everything is clipped to this one number.", default=0.5)
parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
help="print some debug info", default=False)
parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
@@ -341,7 +345,7 @@ class ControlLoopManager:
print("putting it to freedrive for good measure too")
self.robot_manager.rtde_control.freedriveMode()
- if args.save_log:
+ if self.args.save_log:
self.robot_manager.log_manager.saveLog()
# set kill command and join visualization processes
# TODO: actually send them a SIGINT and a SIGKILL if necessary
@@ -470,7 +474,9 @@ class RobotManager:
# we're clipping joint velocities with this.
# if your controllers are not what you expect, you might be commanding a very high velocity,
# which is clipped, resulting in unexpected movement.
- self.max_qd = 0.5 * args.speed_slider
+ self.MAX_QD = 3.14
+ assert args.max_qd <= self.MAX_QD and args.max_qd > 0.0
+ self.max_qd = args.max_qd
self.gripper = None
if (self.args.gripper != "none") and not self.pinocchio_only:
@@ -929,3 +935,9 @@ class RobotManager:
if self.args.debug_prints:
print("terminated manipulator_visualizer_process")
+ def stopRobot(self):
+ if not self.args.pinocchio_only:
+ print("stopping via freedrive lel")
+ self.rtde_control.freedriveMode()
+ time.sleep(0.5)
+ self.rtde_control.endFreedriveMode()
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
similarity index 90%
rename from python/ur_simple_control/optimal_control/optimal_control.py
rename to python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 4da6ba3fd504a8ce2f896bfcfef30e6fb798db3f..152e0f9218da9b6edb1464dd7027143c9dad7415 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -22,7 +22,7 @@ def get_OCP_args(parser : argparse.ArgumentParser):
# TODO: use fddp and incorporate torque (i.e. velocity constraints)
# --> those will correspond to max_qd and acceleration attributes in robotmanager
-def IKOCP(args, robot : RobotManager, goal : pin.SE3):
+def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
# NOTE: idk if this makes any sense, but let's go for it
@@ -41,11 +41,13 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# first 3 are for tracking, state and control regulation
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
+ # cost 1) u residual (actuator cost)
uResidual = crocoddyl.ResidualModelControl(state, state.nv)
+ uRegCost = crocoddyl.CostModelResidual(state, uResidual)
+ # cost 2) x residual (overall amount of movement)
xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
xRegCost = crocoddyl.CostModelResidual(state, xResidual)
- uRegCost = crocoddyl.CostModelResidual(state, uResidual)
- # cost 1) distance to goal -> SE(3) error
+ # cost 3) distance to goal -> SE(3) error
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
state,
# TODO: check if this is the frame we actually want (ee tip)
@@ -54,10 +56,14 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
goal.copy(),
state.nv)
goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
- # cost 2) u residual (actuator cost)
- uResidual = crocoddyl.ResidualModelControl(state, state.nv)
- # cost 3) x residual (overall amount of movement)
- xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+ # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
+ # because that's a part of the state, and we don't know final joint positions)
+ frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("tool0"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
# put these costs into the running costs
runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
runningCostModel.addCost("xReg", xRegCost, 1e-3)
@@ -66,6 +72,7 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# NOTE: shouldn't there be a final velocity = 0 here?
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
terminalCostModel.addCost("uReg", uRegCost, 1e3)
+ terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
# the 4th cost is for defining bounds via costs
# NOTE: could have gotten the same info from state.lb and state.ub.
@@ -153,7 +160,7 @@ if __name__ == "__main__":
robot.q = pin.randomConfiguration(robot.model)
goal = pin.SE3.Random()
goal.translation = np.random.random(3) * 0.4
- reference, solver = IKOCP(args, robot, goal)
+ reference, solver = CrocoIKOCP(args, robot, goal)
# we only work within -pi - pi (or 0-2pi idk anymore)
#reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
log = solver.getCallbacks()[1]
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index f47591d995636eee4dd219f3004ed75c1e0574b0..fe80f5e9d71c11753cadb1118869190383545e4c 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc differ
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 240ee89c04d2fca703a08f0ec3f80304f4b696c7..cdb303e8174d784085e917dde458ea4c3b6b14d9 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
diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
index 6682326d87da63d4aff4d17667a11075916bab25..9537c1573b4cd6ace510792eeb22e1f23c9d07f1 100644
--- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
+++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
@@ -76,34 +76,36 @@ class ManipulatorComparisonManager:
self.manipulator_visualizer_ack_queue.get()
###########################################
- # in case you will want log plotter too #
+ # in case you will want log plotters too #
###########################################
- self.log_plotter = False
- self.log_plotter_cmd_queue = None
- self.log_plotter_ack_queue = None
- self.log_plotter_process = None
+ self.log_plotters = []
# NOTE i assume what you want to plot is a time-indexed with
# the same frequency that the control loops run in.
# if it's not then it's pointless to have a running plot anyway.
+ # TODO: put these in a list so that we can have multiple plots at the same time
+ class RunningPlotter:
+ def __init__(self, args, log, log_plotter_time_start, log_plotter_time_stop):
+ self.time_start = log_plotter_time_start
+ self.time_stop = log_plotter_time_stop
+ self.cmd_queue = Queue()
+ self.ack_queue = Queue()
+ self.process = Process(target=logPlotter,
+ args=(args, log, self.cmd_queue,
+ self.ack_queue))
+ self.process.start()
+ self.ack_queue.get()
+
def createRunningPlot(self, log, log_plotter_time_start, log_plotter_time_stop):
- self.log_plotter = True
- self.log_plotter_time_start = log_plotter_time_start
- self.log_plotter_time_stop = log_plotter_time_stop
- self.log_plotter_cmd_queue = Queue()
- self.log_plotter_ack_queue = Queue()
- self.log_plotter_process = Process(target=logPlotter,
- args=(args, log, self.log_plotter_cmd_queue,
- self.log_plotter_ack_queue))
- self.log_plotter_process.start()
- self.log_plotter_ack_queue.get()
+ self.log_plotters.append(self.RunningPlotter(self.args, log, log_plotter_time_start, log_plotter_time_stop))
def updateViz(self, q1, q2, time_index):
self.manipulator_visualizer_cmd_queue.put((q1, q2))
- if self.log_plotter and (time_index >= self.log_plotter_time_start) and (time_index < self.log_plotter_time_stop):
- self.log_plotter_cmd_queue.put(time_index - self.log_plotter_time_start)
- self.log_plotter_ack_queue.get()
+ for log_plotter in self.log_plotters:
+ if (time_index >= log_plotter.time_start) and (time_index < log_plotter.time_stop):
+ log_plotter.cmd_queue.put(time_index - log_plotter.time_start)
+ log_plotter.ack_queue.get()
self.manipulator_visualizer_ack_queue.get()
# NOTE: this uses slightly fancy python to make it bareable to code
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 9890be02c686b6795c4ef457de2f777797ec3314..6d1cfde6bbbbe4a4a7780e4fc7713f79819658d6 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -107,17 +107,13 @@ def realTimePlotter(args, queue):
colors = plt.cm.jet(np.linspace(0, 1, log_item[data_key].shape[0]))
ax = fig.add_subplot(int(subplot_col_row + str(i + 1)))
# some hacks, i'll have to standardize things somehow
- if data_key == 'qs':
+ if 'qs' in data_key:
ax.set_ylim(bottom=-6.14, top=6.14)
- if data_key == 'dmp_poss':
- ax.set_ylim(bottom=-6.14, top=6.14)
- if data_key == 'dqs':
- ax.set_ylim(bottom=-1.7, top=1.7)
- if data_key == 'dmp_vels':
- ax.set_ylim(bottom=-1.7, top=1.7)
+ if 'vs' in data_key:
+ ax.set_ylim(bottom=-3.14, top=3.14)
if 'wrench' in data_key:
ax.set_ylim(bottom=-20.0, top=20.0)
- if data_key == 'tau':
+ if 'tau' in data_key:
ax.set_ylim(bottom=-2.0, top=2.0)
axes_and_updating_artists[data_key] = AxisAndArtists(ax, {})
for j in range(log_item[data_key].shape[0]):
@@ -302,7 +298,13 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
viz.viewer.window.server_proc.wait()
-
+# TODO: this has to be a class so that we can access
+# the canvas in the onclik event function
+# because that can be a method of that class and
+# get arguments that way.
+# but otherwise i can't pass arguments to that.
+# even if i could with partial, i can't return them,
+# so no cigar from that
def logPlotter(args, log, cmd_queue, ack_queue):
"""
logPlotter
@@ -353,16 +355,30 @@ def logPlotter(args, log, cmd_queue, ack_queue):
axes_and_updating_artists[data_key] = AxisAndArtists(ax, point_in_time_line)
axes_and_updating_artists[data_key].ax.legend(loc='upper left')
- # need to call it once
+ # need to call it once to start, more if something breaks
canvas.draw()
canvas.flush_events()
background = canvas.copy_from_bbox(fig.bbox)
+ # we need to have an event that triggers redrawing
+ #cid = fig.canvas.mpl_connect('button_press_event', onclick)
+ def onEvent(event):
+ print("drawing")
+ canvas.draw()
+ canvas.flush_events()
+ background = canvas.copy_from_bbox(fig.bbox)
+ print("copied canvas")
+
+ cid = fig.canvas.mpl_connect('button_press_event', onEvent)
+
+
ack_queue.put("ready")
if args.debug_prints:
print("LOG_PLOTTER: FULLY ONLINE")
try:
+ counter = 0
while True:
+ counter += 1
time_index = cmd_queue.get()
if time_index == "befree":
if args.debug_prints:
@@ -372,8 +388,18 @@ def logPlotter(args, log, cmd_queue, ack_queue):
for data_key in log:
axes_and_updating_artists[data_key].artists.set_xdata([time_index])
axes_and_updating_artists[data_key].ax.draw_artist(axes_and_updating_artists[data_key].artists)
- canvas.blit(fig.bbox)
- canvas.flush_events()
+ # NOTE: this is stupid, i just want to catch the resize event
+ if not (counter % 50 == 0):
+ #if True:
+ print(counter)
+ canvas.blit(fig.bbox)
+ canvas.flush_events()
+ else:
+ print("drawing")
+ canvas.draw()
+ canvas.flush_events()
+ background = canvas.copy_from_bbox(fig.bbox)
+ print("copied canvas")
ack_queue.put("ready")
except KeyboardInterrupt:
if args.debug_prints: