diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index b59fcce0499fab94884093e38ecf948933a2717a..eebd716670a2552ef6b2850384768b390ae8cf3d 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 e1ec0d0b99d4fb96948b9ab5c2c7409a97300de5..1faaaed581604ec4087a48d6b79a04fa2e648a92 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 aeb95345ce0ab563d438da95c9d10e2bc1e0b472..c8f48d08b458601ee23f36b92d2cfcec641dd81a 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -120,14 +120,17 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
- error_q = q_ref - q
+ if robot.robot_name == "ur5e":
+ error_q = q_ref - q
+ if robot.robot_name == "heron":
+ error_q = pin.difference(robot.model, q, q_ref) #/ robot.dt
error_v = v_ref - v
Kp = 1.0
Kd = 0.5
# feedforward feedback
v_cmd = v_ref + Kp * error_q #+ Kd * error_v
- qd_cmd = v_cmd[:6]
+ #qd_cmd = v_cmd[:6]
robot.sendQd(v_cmd)
log_item['error_qs'] = error_q
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index dfffff597f4062b3daf7d7f7ffaec21bea628a1c..a23fba31207a3e68da78bce780f81d1f34983734 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,12 +1,5 @@
# TODO rename all private variables to start with '_'
-# TODO: neither visualization works without printing from manager,
-# make this make sense dude (but it does work with that, so this aint prio)
-# TODO: just read the q and update everything every timestep, don't deepcopy,
-# TODO: rewrite all getSomething functions to updateSomething functions,
-# and then make the getSomething function just be return self.something.copy()
-# --> just create a RobotManager.step() function, update everything there
-# don't do forwardKinematics 2 extra times for no good reason. make that the libraries
-# responsibility, not the users
+# TODO: make importing nicer with __init__.py files
import pinocchio as pin
import numpy as np
import time
@@ -17,12 +10,13 @@ from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripp
from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
import copy
import signal
-from ur_simple_control.util.get_model import get_model
+from ur_simple_control.util.get_model import get_model, heron_approximation
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
from multiprocessing import Process, Queue
import argparse
+from sys import exc_info
"""
general notes
@@ -75,6 +69,9 @@ def getMinimalArgParser():
#################################################
# general arguments: connection, plotting etc #
#################################################
+ parser.add_argument('--robot', type=str, \
+ help="which robot you're running or simulating", default="ur5e", \
+ choices=['ur5e', 'heron'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -273,7 +270,8 @@ class ControlLoopManager:
# don't send what wasn't ready
if self.args.visualize_manipulator:
if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
- self.robot_manager.manipulator_visualizer_queue.put_nowait(self.robot_manager.q)
+ self.robot_manager.manipulator_visualizer_queue.put_nowait({"q" : self.robot_manager.q,
+ "T_w_e" : self.robot_manager.getT_w_e()})
# if self.args.debug_prints:
# print("manipulator_visualizer_queue size status:", self.robot_manager.manipulator_visualizer_queue.qsize())
if self.args.real_time_plotting:
@@ -334,7 +332,7 @@ class ControlLoopManager:
"""
print('sending 300 speedjs full of zeros and exiting')
for i in range(300):
- vel_cmd = np.zeros(6)
+ vel_cmd = np.zeros(self.robot_manager.model.nv)
#self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
self.robot_manager.sendQd(vel_cmd)
# hopefully this actually stops it
@@ -363,7 +361,7 @@ class ControlLoopManager:
if self.args.visualize_manipulator:
if self.args.debug_prints:
print("i am putting befree in manipulator to stop the manipulator visualizer")
- self.robot_manager.manipulator_visualizer_queue.put_nowait("befree")
+ self.robot_manager.manipulator_visualizer_queue.put_nowait({"befree" : "befree"})
#time.sleep(1)
#self.robot_manager.manipulator_visualizer_process.join()
self.robot_manager.manipulator_visualizer_process.terminate()
@@ -421,8 +419,14 @@ class RobotManager:
self.simulation = args.simulation
# load model
# collision and visual models are none if args.visualize == False
- self.model, self.collision_model, self.visual_model, self.data = \
- get_model()
+ self.robot_name = args.robot
+ if self.robot_name == "ur5e":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ get_model()
+ if self.robot_name == "heron":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ heron_approximation()
+
# start visualize manipulator process if selected.
# has to be started here because it lives throughout the whole run
if args.visualize_manipulator:
@@ -529,7 +533,7 @@ class RobotManager:
q = np.array(q)
self.q = q
if args.visualize_manipulator:
- self.manipulator_visualizer_queue.put(q)
+ self.manipulator_visualizer_queue.put({"q" : q})
# do it once to get T_w_e
@@ -816,24 +820,31 @@ class RobotManager:
"""
# we're hiding the extra 2 prismatic joint shenanigans from the control writer
# because there you shouldn't need to know this anyway
- qd_cmd = qd[:6]
- # np.clip is ok with bounds being scalar, it does what it should
- # (but you can also give it an array)
- qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
- if not self.pinocchio_only:
- # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
- self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
- else:
- # this one takes all 8 elements of qd since we're still in pinocchio
- # this is ugly, todo: fix
- qd = qd[:6]
- qd = qd_cmd.reshape((6,))
- qd = list(qd)
- qd.append(0.0)
- qd.append(0.0)
- qd = np.array(qd)
- self.v_q = qd
- self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ if self.robot_name == "ur5e":
+ qd_cmd = qd[:6]
+ # np.clip is ok with bounds being scalar, it does what it should
+ # (but you can also give it an array)
+ qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
+ if not self.pinocchio_only:
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ else:
+ # this one takes all 8 elements of qd since we're still in pinocchio
+ # this is ugly, todo: fix
+ qd = qd[:6]
+ qd = qd_cmd.reshape((6,))
+ qd = list(qd)
+ qd.append(0.0)
+ qd.append(0.0)
+ qd = np.array(qd)
+ self.v_q = qd
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+ # TODO: implement real thing
+ if self.robot_name == "heron":
+ self.v_q = qd
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
def openGripper(self):
if self.gripper is None:
@@ -901,7 +912,7 @@ class RobotManager:
for i in range(len(goal_list)):
goal_list[i] = float(goal_list[i])
except:
- e = sys.exc_info()
+ e = exc_info()
print("The input is not in the expected format. Try again.")
print(e)
if e == "ok":
@@ -912,6 +923,10 @@ class RobotManager:
# NOTE i'm not deepcopying this on purpose
# but that might be the preferred thing, we'll see
self.Mgoal = Mgoal
+ if self.args.visualize_manipulator:
+ # TODO document this somewhere
+ self.manipulator_visualizer_queue.put(
+ {"Mgoal" : Mgoal})
return Mgoal
def killManipulatorVisualizer(self):
@@ -929,7 +944,7 @@ class RobotManager:
if self.args.debug_prints:
print("i am putting befree in plotter_queue to stop the manipulator visualizer")
# putting this command tells our process to kill the meshcat zmq server process
- self.manipulator_visualizer_queue.put_nowait("befree")
+ self.manipulator_visualizer_queue.put_nowait({"befree" : "befree"})
time.sleep(0.1)
self.manipulator_visualizer_process.terminate()
if self.args.debug_prints:
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 152e0f9218da9b6edb1464dd7027143c9dad7415..8faf38ad94f7a9e28f5c3a89ff729de346cd8c1d 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -20,8 +20,6 @@ def get_OCP_args(parser : argparse.ArgumentParser):
return parser
-# TODO: use fddp and incorporate torque (i.e. velocity constraints)
-# --> those will correspond to max_qd and acceleration attributes in robotmanager
def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
@@ -87,9 +85,14 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
- limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
- runningCostModel.addCost("limitCost", limitCost, 1e3)
- terminalCostModel.addCost("limitCost", limitCost, 1e3)
+
+ # TOOODOOO ADD THE SAME FOR WHOLE HERON
+ if robot.robot_name == "ur5e":
+ limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
+ runningCostModel.addCost("limitCost", limitCost, 1e3)
+ terminalCostModel.addCost("limitCost", limitCost, 1e3)
+
+
# NOTE: i have no idea how to incorporate this into anything
# we need to add state constraints for things to make sense
# NOTE!!!: there are no hard inequality constraints on states
@@ -158,6 +161,9 @@ if __name__ == "__main__":
robot.max_qd = 3.14
print("velocity limits", robot.model.velocityLimit)
robot.q = pin.randomConfiguration(robot.model)
+ robot.q[0] = 0.1
+ robot.q[1] = 0.1
+ print(robot.q)
goal = pin.SE3.Random()
goal.translation = np.random.random(3) * 0.4
reference, solver = CrocoIKOCP(args, robot, goal)
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 4f3cd82c660184ddd923df726a18a1539e1dba3b..7fb311856ef9a0ccfa2de4ba05ba86e1b0356785 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 2d624babbace54bee57892bd25692f37c833fd5f..a5737ee1084517f9f6c1a1550e1d6d8680df75c6 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -9,6 +9,7 @@ import numpy as np
import sys
import os
from importlib.resources import files
+import hppfcl as fcl
# can't get the urdf reading with these functions to save my life, idk what or why
@@ -83,3 +84,80 @@ def get_model():
return model, collision_model, visual_model, data
+# this gives me a flying joint for the camera,
+# and a million joints for wheels -> it's unusable
+# TODO: look what's done in pink, see if this can be usable
+# after you've removed camera joint and similar.
+def get_heron_model():
+
+ #urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
+ urdf_path_absolute = "/home/gospodar/home2/gospodar/lund/praxis/software/ros/ros-containers/home/model.urdf"
+ #mesh_dir = files('ur_simple_control')
+ #mesh_dir_absolute = os.path.abspath(mesh_dir)
+ mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
+
+ 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)
+
+ data = pin.Data(model)
+
+ return model, collision_model, visual_model, data
+
+def heron_approximation():
+ # arm + gripper
+ model_arm, collision_model_arm, visual_model_arm, data_arm = get_model()
+
+ # mobile base as planar joint (there's probably a better
+ # option but whatever right now)
+ model_mobile_base = pin.Model()
+ model_mobile_base.name = "mobile_base"
+ geom_model_mobile_base = pin.GeometryModel()
+ joint_name = "mobile_base_planar_joint"
+ parent_id = 0
+ # TEST
+ joint_placement = pin.SE3.Identity()
+ #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ #joint_placement.translation[2] = 0.2
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(),
+ joint_placement.copy(), joint_name)
+ #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # box_dimensions[1], box_dimensions[2])
+
+ # pretty much random numbers
+ # TODO: find heron (mir) numbers
+ body_inertia = pin.Inertia.FromBox(30, 0.5,
+ 0.3, 0.4)
+ # maybe change placement to sth else depending on where its grasped
+ model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity())
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
+ body_placement = pin.SE3.Identity()
+ geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy())
+
+ geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ model_mobile_base.addFrame(pin.Frame('base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+ # frame-index should be 1
+ model, visual_model = pin.appendModel(model_mobile_base, model_arm, geom_model_mobile_base, visual_model_arm, 1, pin.SE3.Identity())
+ data = model.createData()
+
+ # fix gripper
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ geom.meshcolor = np.array([1.0, 0.1, 0.1, 1.0])
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+
+ return model, visual_model.copy(), visual_model, data
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 eaf4ae636a0f3e0494bc8ae3166c0c46fe1bf886..7039720b6b1d9858d08e7c3911c57c5324a85ff2 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/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 8c787350b82b417d6e13112fcd3f0d18e666fea6..0215e599f385bc8268641ecaa48baa462c7760d8 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -4,11 +4,12 @@ from collections import deque, namedtuple
import time
import copy
from pinocchio.visualize import MeshcatVisualizer
+import meshcat_shapes
-# tkinter stuff
-from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk)
-from tkinter import *
-from tkinter.ttk import *
+# tkinter stuff for later reference
+#from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk)
+#from tkinter import *
+#from tkinter.ttk import *
# rows and cols are flipped lel
def getNRowsMColumnsFromTotalNumber(n_plots):
@@ -162,6 +163,10 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
# for whatever reason the hand-e files don't have/
# meshcat can't read scaling information.
# so we scale manually
+ init_target_frame = False
+ init_ee_frame = False
+ # if we go for manipulator end-effector
+ #meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
for geom in visual_model.geometryObjects:
if "hand" in geom.name:
s = geom.meshScale
@@ -180,16 +185,31 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
print("MANIPULATORVISUALIZER: FULLY ONLINE")
try:
while True:
- q = queue.get()
- if type(q) == str:
- print("got str q")
- if q == "befree":
+ cmd = queue.get()
+ for key in cmd:
+ if key == "befree":
if args.debug_prints:
print("MANIPULATORVISUALIZER: got befree, manipulatorVisualizer out")
viz.viewer.window.server_proc.kill()
viz.viewer.window.server_proc.wait()
break
- viz.display(q)
+ if key == "Mgoal":
+ # having this via flag is stupid, but i can't
+ # be bothered with something else rn
+ if init_target_frame == False:
+ meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
+ init_target_frame = True
+ viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
+ if key == "T_w_e":
+ # having this via flag is stupid, but i can't
+ # be bothered with something else rn
+ if init_ee_frame == False:
+ meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
+ init_ee_frame = True
+ viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
+ if key == "q":
+ viz.display(cmd["q"])
+
except KeyboardInterrupt:
if args.debug_prints:
print("MANIPULATORVISUALIZER: caught KeyboardInterrupt, i'm out")