diff --git a/python/examples/clik.py b/python/examples/clik.py
index f066ec8870934cd454dd81387fa071b18f95aa1f..3a32ec245b32caf0e7f670ec1aec9a516bfdd575 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -58,6 +58,8 @@ def get_args():
default='1.0', help='not actually_used atm')
parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
help="print some debug info", default=False)
+ parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
+ help="save log data folder in whatever directory you called this in. if it doesn't exists there, it's saved to /tmp/data", default=False)
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -68,7 +70,7 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- Mgoal = robot.defineGoalPoint()
+ Mgoal = robot.defineGoalPointCLI()
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
log_dict = {
@@ -81,5 +83,6 @@ if __name__ == "__main__":
# something that generates a file name from something in args.
# also save args there
log_dict, final_iteration = loop_manager.run()
- saveLog(log_dict, final_iteration, args)
+ if args.save_log:
+ saveLog(log_dict, final_iteration, args)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index 053fed9e52e9806ecfd57867b690a7f41b92295f..5455df7279181101a5bbc6a30cb325d201481eab 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.pyc
index 6b95c52eca9803e3be3409010974b2933bc51953..9b3a270c6e1c04ea59a98227f6d366babc3201fd 100644
Binary files a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.pyc and b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.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 b20bdc645d0677edc48f5c1d976df050f8be3c54..ba8399c1675a5522e05d691cb9155eefdb023440 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -139,7 +139,7 @@ def controlLoopClik(robot, clik_controller, i, past_data):
# first check whether we're at the goal
SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.goal_error:
+ if np.linalg.norm(err_vector) < robot.args.goal_error:
# print("Convergence achieved, reached destionation!")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
@@ -235,7 +235,7 @@ def moveL(args, robot, goal_point):
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- Mgoal = robot.defineGoalPoint()
+ Mgoal = robot.defineGoalPointCLI()
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 605a788f90a4898c9725d2995fd0bc66d85d143d..716a238d6d44df1c272c3c5decf47050f656e220 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -17,6 +17,7 @@ import signal
from ur_simple_control.util.get_model import get_model
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict
+from pinocchio.visualize import MeshcatVisualizer
"""
general notes
@@ -156,6 +157,12 @@ class ControlLoopManager:
#self.robot_manager.stopHandler(None, None)
self.log_dict[key][i] = log_entry_dict[key]
+ # TODO: offload this to a different
+ # process, it's hindering real-time performance (not by a lot,
+ # but it does)
+ if self.args.visualize:
+ if i % 20 == 0:
+ self.robot_manager.viz.display(self.robot_manager.q)
# break if done
if breakFlag:
break
@@ -168,7 +175,7 @@ class ControlLoopManager:
else:
time.sleep(self.robot_manager.dt - diff)
self.final_iteration = i
- if args.debug_prints:
+ if self.args.debug_prints:
if i < self.max_iterations -1:
print("success in", i, "iterations!")
else:
@@ -232,13 +239,24 @@ class RobotManager:
# load model
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
- get_model(args.visualize)
+ get_model()
+ # we're using meshcat exclusively.
+ # there are no good options,
+ # but this does work and isn't a dead project
if args.visualize:
- # ---> TODO: write your own vedo 3D visualzier,
- # connect it with the real-time matplotlib line plots,
- # and add some basic gui control (show/don't show stuff,
- # define a goal point, choose controller etc)
- pass
+ # for whatever reason the hand-e files don't have/
+ # meshcat can't read scaling information.
+ # so we scale manually
+ for geom in self.visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+ self.viz = MeshcatVisualizer(self.model, self.collision_model, self.visual_model)
+ self.viz.initViewer(open=True)
+ self.viz.loadViewerModel()
+
# TODO: make general
if self.gripper_flag and not self.pinocchio_only:
@@ -545,7 +563,7 @@ class RobotManager:
"""
- defineGoalPoint
+ defineGoalPointCLI
------------------
--> 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
@@ -557,45 +575,41 @@ class RobotManager:
but also you do want to have both options. obviously you go for the sliders
in the case you're visualizing, makes no sense otherwise.
"""
- def defineGoalPoint(self):
+ def defineGoalPointCLI(self):
q = self.getQ()
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))
T_w_e = self.data.oMi[self.JOINT_ID]
- if not self.args.visualize:
- print("You can only specify the translation right now.")
- if not self.pinocchio_only:
- print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
- print("Here's where the robot is currently. Ensure you know what the base frame is first.")
- print("base frame end-effector pose from pinocchio:\n", \
- *self.data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4))
- print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
- # remain with the current orientation
- # TODO: add something, probably rpy for orientation because it's the least number
- # of numbers you need to type in
- Mgoal = T_w_e.copy()
- # this is a reasonable way to do it too, maybe implement it later
- #Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
- # do a while loop until this is parsed correctly
- while True:
- goal = input("Please enter the target end-effector position in the x.x,y.y,z.z format: ")
- try:
- e = "ok"
- goal_list = goal.split(',')
- for i in range(len(goal_list)):
- goal_list[i] = float(goal_list[i])
- except:
- e = sys.exc_info()
- print("The input is not in the expected format. Try again.")
- print(e)
- if e == "ok":
- Mgoal.translation = np.array(goal_list)
- break
- print("this is goal pose you defined:\n", Mgoal)
- else:
- raise NotImplementedError('Paths in the urdf or something else need to \
- be fixed to use this first. Also need to program in the sliders \
- and visualizing the goal frame. Sorry. Coming soon.')
+ print("You can only specify the translation right now.")
+ if not self.pinocchio_only:
+ print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
+ print("Here's where the robot is currently. Ensure you know what the base frame is first.")
+ print("base frame end-effector pose from pinocchio:\n", \
+ *self.data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4))
+ print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
+ # remain with the current orientation
+ # TODO: add something, probably rpy for orientation because it's the least number
+ # of numbers you need to type in
+ Mgoal = T_w_e.copy()
+ # this is a reasonable way to do it too, maybe implement it later
+ #Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # do a while loop until this is parsed correctly
+ while True:
+ goal = input("Please enter the target end-effector position in the x.x,y.y,z.z format: ")
+ try:
+ e = "ok"
+ goal_list = goal.split(',')
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = sys.exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+
# NOTE i'm not deepcopying this on purpose
# but that might be the preferred thing, we'll see
self.Mgoal = Mgoal
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 177620f1965e84d10ab4a264f60aa2384ae543f1..625e9afb9c9bf6ff072333dee008f431cf9c1eb8 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc
index 759cafe5cf649d89b5731ceaa655e38fdaa3a52f..11e5dcc03d67ce58afde5d3306cdc033b3d7633e 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 452c0d301cdc5af0139a788ad73d13c18894a4dd..2d624babbace54bee57892bd25692f37c833fd5f 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -13,24 +13,29 @@ from importlib.resources import files
# can't get the urdf reading with these functions to save my life, idk what or why
#############################################################
-# PACKAGE_DIR IS THE WHOLE GIT REPO
-# PACKAGE:// IS WHAT'S BEING REPLACED WITH THE PACKAGE_DIR ARGUMENT
-# TODO: REWRITE URDFS TO THAT THEY FOLLOW THIS
-# YOU GIVE ABSOLUTE PATH TO URDF THO.
+# PACKAGE_DIR IS THE WHOLE UR_SIMPLE_CONTROL FOLDER (cos that's accessible from anywhere it's installed)
+# PACKAGE:// IS WHAT'S BEING REPLACED WITH THE PACKAGE_DIR ARGUMENT IN THE URDF.
+# YOU GIVE ABSOLUTE PATH TO THE URDF THO.
#############################################################
-
-def get_model(visualize):
+"""
+loads what needs to be loaded.
+calibration for the particular robot was extracted from the yml
+obtained from ros and appears here as magic numbers.
+i have no idea how to extract calibration data without ros
+and i have no plans to do so.
+aligning what UR thinks is the world frame
+and what we think is the world frame is not really necessary,
+but it does aleviate some brain capacity while debugging.
+having that said, supposedly there is a big missalignment (few cm)
+between the actual robot and the non-calibrated model.
+NOTE: this should be fixed for a proper release
+"""
+def get_model():
- #urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
- # NOTE THIS ONE WORKS IF YOU'RE NOT LOADING VISUAL STUFF
- #urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande.urdf')
- #urdf_path_relative = files('ur_simple_control.robot_descriptions').joinpath('ur5e_with_robotiq_hande.urdf')
urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- #mesh_dir = "../robot_descriptions/"
mesh_dir = files('ur_simple_control')
- #mesh_dir_absolute = os.path.abspath(mesh_dir)
mesh_dir_absolute = os.path.abspath(mesh_dir)
shoulder_trans = np.array([0, 0, 0.1625134425523304])
@@ -57,33 +62,17 @@ def get_model(visualize):
wrist_3_rpy = np.array([1.572215514545703, 3.141592653589793, 3.141592633687631])
wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy),wrist_3_trans)
- # TODO fix paths via python packaging, get this to work
- if visualize:
- #raise NotImplementedError('Paths in the urdf or something else need to be fixed to use this first. Sorry. Coming soon.')
- model = None
- collision_model = None
- visual_model = None
- print("urdf_path_absolute", type(urdf_path_absolute), urdf_path_absolute)
- print("mesh_dir", type(mesh_dir_absolute), mesh_dir_absolute)
- #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
- #print(urdf_path_absolute)
- model = pin.buildModelFromUrdf(urdf_path_absolute)
- visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
- collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
- #visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
- #collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION)
- #geom_model = None
- #package_dir = None
- #mesh_loader = None
- #visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, geom_model, package_dirs=mesh_dir_absolute)
- #visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, package_dirs=mesh_dir_absolute)
- #collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION)
- #data = pin.Data(model)
- else:
- model = pin.buildModelFromUrdf(urdf_path_absolute)
- collision_model = None
- visual_model = None
+ model = None
+ collision_model = None
+ visual_model = None
+ # this command just calls the ones below it. both are kept here
+ # in case pinocchio people decide to change their api.
+ #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
+ model = pin.buildModelFromUrdf(urdf_path_absolute)
+ visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
+ collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
+ # updating joint placements.
model.jointPlacements[1] = shoulder_se3
model.jointPlacements[2] = upper_arm_se3
model.jointPlacements[3] = forearm_se3
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index c8f0c5d516ae35675b8624b16abef47998bc452b..99d4b3e8878c1095d2ea9d7677c870e510130991 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -1,7 +1,7 @@
import pickle
import numpy as np
+import os
-# we're not
def saveLog(log_dict, final_iteration, args):
# shave off the zeros, noone needs 'em
for key in log_dict:
@@ -10,8 +10,13 @@ def saveLog(log_dict, final_iteration, args):
# - generate name based on args + add index
# - you need to run shell ls to get the index,
# there's at least one chalmers project with code for that
- run_file_path = "./data/clik_run_001.pickle"
- args_file_path = "./data/clik_run_001_args.pickle"
+ if os.path.exists('./data'):
+ run_file_path = "./data/clik_run_001.pickle"
+ args_file_path = "./data/clik_run_001_args.pickle"
+ else:
+ os.makedirs('/tmp/data', exist_ok=True)
+ run_file_path = "/tmp/data/clik_run_001.pickle"
+ args_file_path = "/tmp/data/clik_run_001_args.pickle"
# save the logged data
# you need to open it binary for pickling
log_file = open(run_file_path, 'wb')
diff --git a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
index b4b9380a88a1498e52b9345abd85ba72d4b23ce0..59952dc1d02ab557f1ff176a436e9bb7495d3802 100644
--- a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
+++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
@@ -151,8 +151,8 @@ class ManipulatorVisualMotionAnalyzer:
# dicts not because they have to be, but just so that
# they are named because that might be useful
# and because names are the only thing keeping horrendous this code together
- # TODO: current evil sharing of pointers (thanks python lmao) needs to be deleted
- # ofc delete from ik_env, retain here (they're in both spots rn)
+ # TODO: current evil sharing of pointers (thanks python lmao) needs to be deleted
+ # ofc delete from ik_env, retain here (they're in both spots rn)
# NOTE: you need to do ax.draw(artist) for blitting. so each artist needs to be connected
# to its particular artist. so they can't all be in one dict.
# we'll just increase the dict depth by 1, and add a named tuple to layer 1 (layer of axis)
diff --git a/python/ur_simple_control/visualize/vedo_manipulator.py b/python/ur_simple_control/visualize/vedo_manipulator.py
index ec5c249280683eb21b97b25a60de2a5b4d0996b8..d4a2a38726554c3231b7299b71d7f1253b06cd33 100644
--- a/python/ur_simple_control/visualize/vedo_manipulator.py
+++ b/python/ur_simple_control/visualize/vedo_manipulator.py
@@ -65,15 +65,6 @@ def get_args():
#######################################################################
-def drawStateAnim(self):
- toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])]
- drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)), self.avg_link_lenth * 2)
- for j in range(len(self.joints)):
- toBase.append(toBase[-1] @ self.joints[j].HomMat)
- drawOrientationAnim(self.ax, toBase[-1][0:3,0:3], self.lines[j][:-1], toBase[-1][0:3,3], self.avg_link_lenth)
- drawVectorAnim(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), self.lines[j][-1], toBase[-2][0:3,3],self.color_link)
-
-
if __name__ == "__main__":
pin.seed(int(time.time()))
args = get_args()
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 9e8c4677057aac406452ba5de2875c71a717da25..4c7f71e165100d56a01d20fcd83d075e6847334e 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -1,10 +1,13 @@
import numpy as np
import matplotlib.pyplot as plt
-# TODO let's try to make this general
-# make plot_data a dictionary.
-# every key is one subplot, and it's value
-# is what you plot
+"""
+plotFromDict
+------------
+plots logs stored in a dictionary
+- every key is one subplot, and it's value
+ is what you plot
+"""
def plotFromDict(plot_data, final_iteration, args):
# TODO cut zeros from data
# TODO: replace with actual time
@@ -25,3 +28,11 @@ def plotFromDict(plot_data, final_iteration, args):
ax_dict[data_key].plot(t, plot_data[data_key][:final_iteration,j], color=colors[j], label=data_key + "_" + str(j))
ax_dict[data_key].legend()
plt.show()
+
+
+# you need to write out specifications for this.
+# only then implement.
+# this is too fucked to try to do it from your head
+#class RealTimePlotter:
+# def __init__(self, TODO):
+