diff --git a/python/smc/util/ft_calibration.py b/python/examples/old_or_experimental/ft_calibration_via_amperes_test.py
similarity index 100%
rename from python/smc/util/ft_calibration.py
rename to python/examples/old_or_experimental/ft_calibration_via_amperes_test.py
diff --git a/python/smc/__init__.py b/python/smc/__init__.py
index 80bfd4616a268b4b234dd7f7f0b6e0311a547422..d65ce69d29bf97e05e24deebb21056ec351711a8 100644
--- a/python/smc/__init__.py
+++ b/python/smc/__init__.py
@@ -3,17 +3,22 @@ from smc import (
control,
multiprocessing,
util,
- visualize,
+ visualization,
vision,
path_generation,
+ logging
)
+from smc.robots.utils import getMinimalArgParser
+
__all__ = [
+ "getMinimalArgParser",
"robots",
"control",
"multiprocessing",
"util",
- "visualize",
+ "visualization",
"vision",
"path_generation",
+ "logging",
]
diff --git a/python/smc/control/basics/__init__.py b/python/smc/control/basics/__init__.py
deleted file mode 100644
index 71b8f3e97aad55efaea5bc8f6fa6a2ed5c4669a4..0000000000000000000000000000000000000000
--- a/python/smc/control/basics/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from .basics import *
diff --git a/python/smc/control/basics/basics.py b/python/smc/control/basics/basics.py
deleted file mode 100644
index baf51a28097523467884b89a3f35003d05986a50..0000000000000000000000000000000000000000
--- a/python/smc/control/basics/basics.py
+++ /dev/null
@@ -1,384 +0,0 @@
-# this is a quickest possible solution,
-# not a good one
-# please change at the earliest convenience
-
-import pinocchio as pin
-import numpy as np
-import copy
-import argparse
-from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-import time
-import threading
-import queue
-
-def moveJControlLoop(q_desired, robot : RobotManager, i, past_data):
- """
- moveJControlLoop
- ---------------
- most basic P control for joint space point-to-point motion, actual control loop.
- """
- breakFlag = False
- save_past_dict = {}
- # you don't even need forward kinematics for this lmao
- q = robot.getQ()
- # TODO: be more intelligent with qs
- q = q[:6]
- q_desired = q_desired[:6]
- q_error = q_desired - q
-
- # STOP MUCH BEFORE YOU NEED TO FOR THE DEMO
- # EVEN THIS MIGHT BE TOO MUCH
- # TODO fix later obviously
- if np.linalg.norm(q_error) < 1e-3:
- breakFlag = True
- # stupid hack, for the love of god remove this
- # but it should be small enough lel
- # there. fixed. tko radi taj i grijesi, al jebemu zivot sta je to bilo
- K = 120
- #print(q_error)
- # TODO: you should clip this
- qd = K * q_error * robot.dt
- #qd = np.clip(qd, robot.acceleration, robot.acceleration)
- robot.sendQd(qd)
- return breakFlag, {}, {}
-
-# TODO:
-# fix this by tuning or whatever else.
-# MOVEL works just fine, so apply whatever's missing for there here
-# and that's it.
-def moveJP(args, robot, q_desired):
- """
- moveJP
- ---------------
- most basic P control for joint space point-to-point motion.
- just starts the control loop without any logging.
- """
- assert type(q_desired) == np.ndarray
- controlLoop = partial(moveJControlLoop, q_desired, robot)
- # we're not using any past data or logging, hence the empty arguments
- loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
- loop_manager.run()
- # TODO: remove, this isn't doing anything
- #time.sleep(0.01)
- if args.debug_prints:
- 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 followKinematicJointTrajPControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
- breakFlag = False
- save_past_dict = {}
- log_item = {}
- q = robot.getQ()
- v = robot.getQd()
- # NOTE: assuming we haven't missed a timestep,
- # which is pretty much true
- t = i * robot.dt
- # take the future (next) one
- t_index_lower = int(np.floor(t / reference['dt']))
- t_index_upper = int(np.ceil(t / reference['dt']))
-
- # TODO: set q_refs and v_refs once (merge with interpolation if)
- 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(robot.model.nv)
- else:
- v_ref = reference['vs'][t_index_upper]
-
- # 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
-
- # TODO: move interpolation to a different function later
- 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)
-
-
- # TODO: why not use pin.difference for both?
- 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]
- robot.sendQd(v_cmd)
-
- 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
-
-def followKinematicJointTrajP(args, robot, reference):
- # we're not using any past data or logging, hence the empty arguments
- controlLoop = partial(followKinematicJointTrajPControlLoop, args.stop_at_final, robot, reference)
- log_item = {
- '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()
- if args.debug_prints:
- print("followKinematicJointTrajP done: reached path destionation!")
-
-
-
-def moveJPIControlLoop(q_desired, robot : RobotManager, i, past_data):
- """
- PID control for joint space point-to-point motion with approximated joint velocities.
- """
-
- # ================================
- # Initialization
- # ================================
- breakFlag = False
- save_past_dict = {}
- log_item = {}
-
- # ================================
- # Current Joint Positions
- # ================================
- q = robot.getQ()[:6] # Get current joint positions (first 6 joints)
-
- # ================================
- # Retrieve Previous States
- # ================================
- q_prev = past_data['q_prev'][-1] # Previous joint positions
- e_prev = past_data['e_prev'][-1] # Previous position error
-
- qd_actual = robot.getQd()[:6]
-
- # ================================
- # Compute Position Error
- # ================================
- q_error = q_desired - q # Position error
-
-
- # ================================
- # Check for Convergence
- # ================================
- if np.linalg.norm(q_error) < 1e-3 and np.linalg.norm(qd_actual) < 1e-3:
- breakFlag = True # Convergence achieved
-
- # ================================
- # Update Integral of Error
- # ================================
- integral_error = past_data['integral_error'][-1]
- integral_error = np.array(integral_error, dtype=np.float64).flatten()
- integral_error += q_error * robot.dt # Accumulate error over time
-
- # Anti-windup: Limit integral error to prevent excessive accumulation
- max_integral = 10
- integral_error = np.clip(integral_error, -max_integral, max_integral)
-
- # ================================
- # Save Current States for Next Iteration
- # ================================
- save_past_dict['integral_error'] = integral_error # Save updated integral error
- save_past_dict['q_prev'] = q # Save current joint positions
- save_past_dict['e_prev'] = q_error # Save current position error
-
- # ================================
- # Control Gains
- # ================================
- Kp = 7.0 # Proportional gain
- Ki = 0.0 # Integral gain
-
- # ================================
- # Compute Control Input (Joint Velocities)
- # ================================
- qd = Kp * q_error + Ki * integral_error
- #qd[5]=qd[5]*10
-
- # ================================
- # Send Joint Velocities to the Robot
- # ================================
- robot.sendQd(qd)
-
- qd = robot.getQd()
- log_item['qs'] = q
- log_item['dqs'] = qd[:6]
- log_item['integral_error'] = integral_error # Save updated integral error
- log_item['e_prev'] = q_error # Save current position error
- return breakFlag, save_past_dict, log_item
-
-
-def moveJPI(args, robot, q_desired):
- assert isinstance(q_desired, np.ndarray)
- controlLoop = partial(moveJPIControlLoop, q_desired, robot)
-
- # ================================
- # Initialization
- # ================================
- # Get initial joint positions
- initial_q = robot.getQ()[:6]
-
- # Initialize past data for control loop
- save_past_dict = {
- 'integral_error': np.zeros(robot.model.nq)[:6],
- 'q_prev': initial_q,
- 'e_prev': q_desired - initial_q, # Initial position error (may need to be q_desired - initial_q)
- }
-
- # Initialize log item (if logging is needed)
- log_item = {
- 'qs' : np.zeros(6),
- 'dqs' : np.zeros(6),
- 'integral_error': np.zeros(robot.model.nq)[:6],
- 'e_prev' : q_desired - initial_q, # Initial position error (may need to be q_desired - initial_q)
- }
-
- # ================================
- # Create and Run Control Loop Manager
- # ================================
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- loop_manager.run()
-
- # ================================
- # Debug Printing
- # ================================
- if args.debug_prints:
- print("MoveJPI done: convergence achieved, reached destination!")
-
-
-def freedriveControlLoop(args, robot : RobotManager, com_queue, pose_n_q_dict, i, past_data):
- """
- controlLoopFreedrive
- -----------------------------
- while in freedrive, collect qs.
- this can be used to visualize and plot while in freedrive,
- collect some points or a trajectory etc.
- this function does not have those features,
- but you can use this function as template to make them
- """
- breakFlag = False
- log_item = {}
- save_past_dict = {}
-
- q = robot.getQ()
- wrench = robot.getWrench()
- T_w_e = robot.getT_w_e()
-
- if not com_queue.empty():
- msg = com_queue.get()
- if msg == 'q':
- breakFlag = True
- if msg == 's':
- pose_n_q_dict['T_w_es'].append(T_w_e.copy())
- pose_n_q_dict['qs'].append(q.copy())
-
- if args.debug_prints:
- print("===========================================")
- print(T_w_e)
- print("q:", *np.array(q).round(4))
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
-
-def freedriveUntilKeyboard(args, robot : RobotManager):
- """
- controlLoopFreedrive
- -----------------------------
- while in freedrive, collect qs.
- this can be used to visualize and plot while in freedrive,
- collect some points or a trajectory etc.
- you can save the log from this function and use that,
- or type on the keyboard to save specific points (q + T_w_e)
- """
- if args.pinocchio_only:
- print("""
- ideally now you would use some sliders or something,
- but i don't have time to implement that. just run some movement
- to get it where you want pls. freedrive will just exit now
- """)
- return {}
- robot.setFreedrive()
- # set up freedrive controlloop (does nothing, just accesses
- # all controlLoopManager goodies)
- log_item = {'qs' : np.zeros((robot.model.nq,)),
- 'dqs' : np.zeros((robot.model.nv,))
- }
- save_past_dict = {}
- # use queue.queue because we're doing this on a
- # threading level, not multiprocess level
- # infinite size (won't need more than 1 but who cares)
- com_queue = queue.Queue()
- # we're passing pose_n_q_list by reference
- # (python default for mutables)
- pose_n_q_dict = {'T_w_es' : [], 'qs': []}
- controlLoop = ControlLoopManager(robot, partial(freedriveControlLoop, args, robot, com_queue, pose_n_q_dict), args, save_past_dict, log_item)
-
- # wait for keyboard input in a different thread
- # (obviously necessary because otherwise literally nothing else
- # can happen)
- def waitKeyboardFunction(com_queue):
- cmd = ""
- # empty string is cast to false
- while True:
- cmd = input("Press q to stop and exit, s to save joint angle and T_w_e: ")
- if (cmd != "q") and (cmd != "s"):
- print("invalid input, only s or q (then Enter) allowed")
- else:
- com_queue.put(cmd)
- if cmd == "q":
- break
-
- # we definitely want a thread and leverage GIL,
- # because the thread is literally just something to sit
- # on a blocking call from keyboard input
- # (it would almost certainly be the same without the GIL,
- # but would maybe require stdin sharing or something)
- waitKeyboardThread = threading.Thread(target=waitKeyboardFunction, args=(com_queue,))
- waitKeyboardThread.start()
- controlLoop.run()
- waitKeyboardThread.join()
- robot.unSetFreedrive()
- return pose_n_q_dict
-
-if __name__ == "__main__":
- parser = getMinimalArgParser()
- args = parser.parse_args()
- robot = RobotManager(args)
- robot._step()
- print(robot.q)
- q_goal = np.random.random(6) * 2*np.pi - np.pi
- print(q_goal)
- #moveJPI(args, robot, q_goal)
- moveJP(args, robot, q_goal)
- robot.killManipulatorVisualizer()
diff --git a/python/smc/control/clik/clik.py b/python/smc/control/clik/clik.py
index a65d2ed072c002ee1fc48390a465f9dd10569191..4a7141d8d6a2ad21378db68f5ac340feb4f6826f 100644
--- a/python/smc/control/clik/clik.py
+++ b/python/smc/control/clik/clik.py
@@ -676,7 +676,7 @@ def cartesianPathFollowingWithPlannerControlLoop(
# for pp in pathSE3:
# print(pp.translation)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
- if args.visualize_manipulator:
+ if args.visualizer:
robot.visualizer_manager.sendCommand({"path": pathSE3})
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
diff --git a/python/smc/control/freedrive.py b/python/smc/control/freedrive.py
new file mode 100644
index 0000000000000000000000000000000000000000..d7f6e7d45af64ea0fee4d34402bca1e1376c7256
--- /dev/null
+++ b/python/smc/control/freedrive.py
@@ -0,0 +1,113 @@
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.control_loop_manager import ControlLoopManager
+
+import numpy as np
+from functools import partial
+import threading
+import queue
+
+
+def freedriveControlLoop(
+ args, robot: SingleArmInterface, com_queue, pose_n_q_dict, i, past_data
+):
+ """
+ controlLoopFreedrive
+ -----------------------------
+ while in freedrive, collect qs.
+ this can be used to visualize and plot while in freedrive,
+ collect some points or a trajectory etc.
+ this function does not have those features,
+ but you can use this function as template to make them
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.q
+ T_w_e = robot.T_w_e()
+
+ if not com_queue.empty():
+ msg = com_queue.get()
+ if msg == "q":
+ breakFlag = True
+ if msg == "s":
+ pose_n_q_dict["T_w_es"].append(T_w_e.copy())
+ pose_n_q_dict["qs"].append(q.copy())
+
+ if args.debug_prints:
+ print("===========================================")
+ print(T_w_e)
+ print("q:", *np.array(q).round(4))
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.v.reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+
+def freedriveUntilKeyboard(args, robot: SingleArmInterface):
+ """
+ controlLoopFreedrive
+ -----------------------------
+ while in freedrive, collect qs.
+ this can be used to visualize and plot while in freedrive,
+ collect some points or a trajectory etc.
+ you can save the log from this function and use that,
+ or type on the keyboard to save specific points (q + T_w_e)
+ """
+ if args.pinocchio_only:
+ print(
+ """
+ ideally now you would use some sliders or something,
+ but i don't have time to implement that. just run some movement
+ to get it where you want pls. freedrive will just exit now
+ """
+ )
+ return {}
+ robot.setFreedrive()
+ # set up freedrive controlloop (does nothing, just accesses
+ # all controlLoopManager goodies)
+ log_item = {"qs": np.zeros((robot.model.nq,)), "dqs": np.zeros((robot.model.nv,))}
+ save_past_dict = {}
+ # use queue.queue because we're doing this on a
+ # threading level, not multiprocess level
+ # infinite size (won't need more than 1 but who cares)
+ com_queue = queue.Queue()
+ # we're passing pose_n_q_list by reference
+ # (python default for mutables)
+ pose_n_q_dict = {"T_w_es": [], "qs": []}
+ controlLoop = ControlLoopManager(
+ robot,
+ partial(freedriveControlLoop, args, robot, com_queue, pose_n_q_dict),
+ args,
+ save_past_dict,
+ log_item,
+ )
+
+ # wait for keyboard input in a different thread
+ # (obviously necessary because otherwise literally nothing else
+ # can happen)
+ def waitKeyboardFunction(com_queue):
+ cmd = ""
+ # empty string is cast to false
+ while True:
+ cmd = input("Press q to stop and exit, s to save joint angle and T_w_e: ")
+ if (cmd != "q") and (cmd != "s"):
+ print("invalid input, only s or q (then Enter) allowed")
+ else:
+ com_queue.put(cmd)
+ if cmd == "q":
+ break
+
+ # we definitely want a thread and leverage GIL,
+ # because the thread is literally just something to sit
+ # on a blocking call from keyboard input
+ # (it would almost certainly be the same without the GIL,
+ # but would maybe require stdin sharing or something)
+ waitKeyboardThread = threading.Thread(
+ target=waitKeyboardFunction, args=(com_queue,)
+ )
+ waitKeyboardThread.start()
+ controlLoop.run()
+ waitKeyboardThread.join()
+ robot.unSetFreedrive()
+ return pose_n_q_dict
diff --git a/python/smc/control/joint_space/__init__.py b/python/smc/control/joint_space/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..5eff51317610b8d0f17e2404fa6675e21699882c
--- /dev/null
+++ b/python/smc/control/joint_space/__init__.py
@@ -0,0 +1,3 @@
+from .freedrive import *
+from .joint_space_point_to_point import *
+from .joint_space_trajectory_following import *
diff --git a/python/smc/control/joint_space/joint_space_point_to_point.py b/python/smc/control/joint_space/joint_space_point_to_point.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f54db2c31bfa3293dd217ea666403dda6e275b5
--- /dev/null
+++ b/python/smc/control/joint_space/joint_space_point_to_point.py
@@ -0,0 +1,177 @@
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.control.control_loop_manager import ControlLoopManager
+
+import numpy as np
+from functools import partial
+
+
+def moveJControlLoop(q_desired, robot: AbstractRobotManager, i, past_data):
+ """
+ moveJControlLoop
+ ---------------
+ most basic P control for joint space point-to-point motion, actual control loop.
+ """
+ breakFlag = False
+ save_past_dict = {}
+ # you don't even need forward kinematics for this lmao
+ q = robot.q
+ # TODO: be more intelligent with qs
+ q = q[:6]
+ q_desired = q_desired[:6]
+ q_error = q_desired - q
+
+ # STOP MUCH BEFORE YOU NEED TO FOR THE DEMO
+ # EVEN THIS MIGHT BE TOO MUCH
+ # TODO fix later obviously
+ if np.linalg.norm(q_error) < 1e-3:
+ breakFlag = True
+ # stupid hack, for the love of god remove this
+ # but it should be small enough lel
+ # there. fixed. tko radi taj i grijesi, al jebemu zivot sta je to bilo
+ K = 120
+ # print(q_error)
+ # TODO: you should clip this
+ qd = K * q_error * robot.dt
+ # qd = np.clip(qd, robot.acceleration, robot.acceleration)
+ robot.sendVelocityCommand(qd)
+ return breakFlag, {}, {}
+
+
+# TODO:
+# fix this by tuning or whatever else.
+# MOVEL works just fine, so apply whatever's missing for there here
+# and that's it.
+def moveJP(args, robot, q_desired):
+ """
+ moveJP
+ ---------------
+ most basic P control for joint space point-to-point motion.
+ just starts the control loop without any logging.
+ """
+ assert type(q_desired) == np.ndarray
+ controlLoop = partial(moveJControlLoop, q_desired, robot)
+ # we're not using any past data or logging, hence the empty arguments
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
+ loop_manager.run()
+ # TODO: remove, this isn't doing anything
+ # time.sleep(0.01)
+ if args.debug_prints:
+ print("MoveJP done: convergence achieved, reached destionation!")
+
+
+def moveJPIControlLoop(q_desired, robot: AbstractRobotManager, i, past_data):
+ """
+ PID control for joint space point-to-point motion with approximated joint velocities.
+ """
+
+ # ================================
+ # Initialization
+ # ================================
+ breakFlag = False
+ save_past_dict = {}
+ log_item = {}
+
+ # ================================
+ # Current Joint Positions
+ # ================================
+ q = robot.q # Get current joint positions (first 6 joints)
+
+ # ================================
+ # Retrieve Previous States
+ # ================================
+ q_prev = past_data["q_prev"][-1] # Previous joint positions
+ e_prev = past_data["e_prev"][-1] # Previous position error
+
+ # ================================
+ # Compute Position Error
+ # ================================
+ q_error = q_desired - q # Position error
+
+ # ================================
+ # Check for Convergence
+ # ================================
+ if np.linalg.norm(q_error) < 1e-3 and np.linalg.norm(robot.v) < 1e-3:
+ breakFlag = True # Convergence achieved
+
+ # ================================
+ # Update Integral of Error
+ # ================================
+ integral_error = past_data["integral_error"][-1]
+ integral_error = np.array(integral_error, dtype=np.float64).flatten()
+ integral_error += q_error * robot.dt # Accumulate error over time
+
+ # Anti-windup: Limit integral error to prevent excessive accumulation
+ max_integral = 10
+ integral_error = np.clip(integral_error, -max_integral, max_integral)
+
+ # ================================
+ # Save Current States for Next Iteration
+ # ================================
+ save_past_dict["integral_error"] = integral_error # Save updated integral error
+ save_past_dict["q_prev"] = q # Save current joint positions
+ save_past_dict["e_prev"] = q_error # Save current position error
+
+ # ================================
+ # Control Gains
+ # ================================
+ Kp = 7.0 # Proportional gain
+ Ki = 0.0 # Integral gain
+
+ # ================================
+ # Compute Control Input (Joint Velocities)
+ # ================================
+ v_cmd = Kp * q_error + Ki * integral_error
+ # qd[5]=qd[5]*10
+
+ # ================================
+ # Send Joint Velocities to the Robot
+ # ================================
+ robot.sendVelocityCommand(v_cmd)
+
+ log_item["qs"] = q
+ log_item["dqs"] = robot.v
+ log_item["integral_error"] = integral_error # Save updated integral error
+ log_item["e_prev"] = q_error # Save current position error
+ return breakFlag, save_past_dict, log_item
+
+
+def moveJPI(args, robot, q_desired):
+ assert isinstance(q_desired, np.ndarray)
+ controlLoop = partial(moveJPIControlLoop, q_desired, robot)
+
+ # ================================
+ # Initialization
+ # ================================
+ # Get initial joint positions
+ initial_q = robot.getQ()[:6]
+
+ # Initialize past data for control loop
+ save_past_dict = {
+ "integral_error": np.zeros(robot.model.nq)[:6],
+ "q_prev": initial_q,
+ "e_prev": q_desired
+ - initial_q, # Initial position error (may need to be q_desired - initial_q)
+ }
+
+ # Initialize log item (if logging is needed)
+ log_item = {
+ "qs": np.zeros(6),
+ "dqs": np.zeros(6),
+ "integral_error": np.zeros(robot.model.nq)[:6],
+ "e_prev": q_desired
+ - initial_q, # Initial position error (may need to be q_desired - initial_q)
+ }
+
+ # ================================
+ # Create and Run Control Loop Manager
+ # ================================
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ loop_manager.run()
+
+ # ================================
+ # Debug Printing
+ # ================================
+ if args.debug_prints:
+ print("MoveJPI done: convergence achieved, reached destination!")
diff --git a/python/smc/control/joint_space/joint_space_trajectory_following.py b/python/smc/control/joint_space/joint_space_trajectory_following.py
new file mode 100644
index 0000000000000000000000000000000000000000..a787aee06b0521407e3eeeccbc62633ddf159a12
--- /dev/null
+++ b/python/smc/control/joint_space/joint_space_trajectory_following.py
@@ -0,0 +1,112 @@
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.control.control_loop_manager import ControlLoopManager
+from smc import getMinimalArgParser
+
+import pinocchio as pin
+import numpy as np
+from functools import partial
+
+
+# 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 followKinematicJointTrajPControlLoop(
+ stop_at_final: bool, robot: AbstractRobotManager, reference, i, past_data
+):
+ breakFlag = False
+ save_past_dict = {}
+ log_item = {}
+ q = robot.q
+ v = robot.v
+ # NOTE: assuming we haven't missed a timestep,
+ # which is pretty much true
+ t = i * robot.dt
+ # take the future (next) one
+ t_index_lower = int(np.floor(t / reference["dt"]))
+ t_index_upper = int(np.ceil(t / reference["dt"]))
+
+ # TODO: set q_refs and v_refs once (merge with interpolation if)
+ 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(robot.model.nv)
+ else:
+ v_ref = reference["vs"][t_index_upper]
+
+ # 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
+
+ # TODO: move interpolation to a different function later
+ # --> move it to a module called math
+ 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,
+ )
+
+ # TODO: why not use pin.difference for both?
+ 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]
+ robot.sendVelocityCommand(v_cmd)
+
+ 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
+
+
+def followKinematicJointTrajP(args, robot, reference):
+ # we're not using any past data or logging, hence the empty arguments
+ controlLoop = partial(
+ followKinematicJointTrajPControlLoop, args.stop_at_final, robot, reference
+ )
+ log_item = {
+ "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()
+ if args.debug_prints:
+ print("followKinematicJointTrajP done: reached path destionation!")
diff --git a/python/smc/visualize/__init__.py b/python/smc/logging/__init__.py
similarity index 100%
rename from python/smc/visualize/__init__.py
rename to python/smc/logging/__init__.py
diff --git a/python/smc/util/logging_utils.py b/python/smc/logging/logger.py
similarity index 98%
rename from python/smc/util/logging_utils.py
rename to python/smc/logging/logger.py
index 77665ea571824d215c1e0411148172e4822124d5..39b875764c38063dde88df1bb584d9e72b9e9344 100644
--- a/python/smc/util/logging_utils.py
+++ b/python/smc/logging/logger.py
@@ -1,4 +1,4 @@
-from smc.visualize.visualize import plotFromDict
+from smc.visualization.plotters import plotFromDict
import pickle
import numpy as np
@@ -7,7 +7,7 @@ import subprocess
import re
-class LogManager:
+class Logger:
"""
LogManager
----------
diff --git a/python/smc/robots/__init__.py b/python/smc/robots/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..62f1f0d1d6bdaa181ee981e97b00f4eb3468a0c7
--- /dev/null
+++ b/python/smc/robots/__init__.py
@@ -0,0 +1,5 @@
+from .implementations.heron import RealHeronRobotManager
+from .implementations.mir import RealMirRobotManager
+from .implementations.mobile_yumi import RealMobileYumiRobotManager
+from .implementations.simulated_robot import SimulatedRobotManager
+from .implementations.ur5e import RealUR5eRobotManager
diff --git a/python/smc/robots/get_model.py b/python/smc/robots/get_model.py
deleted file mode 100644
index e416db92db9431842d2bbd070056285fe6365d43..0000000000000000000000000000000000000000
--- a/python/smc/robots/get_model.py
+++ /dev/null
@@ -1,558 +0,0 @@
-"""
-possible improvement:
- get the calibration file of the robot without ros
- and then put it here.
- these magic numbers are not a good look.
-"""
-
-import pinocchio as pin
-import numpy as np
-import sys
-import os
-from importlib.resources import files
-import hppfcl as fcl
-import example_robot_data
-
-# can't get the urdf reading with these functions to save my life, idk what or why
-
-#############################################################
-# PACKAGE_DIR IS THE WHOLE smc 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.
-#############################################################
-
-"""
-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 = files("smc.robots.robot_descriptions.urdf").joinpath(
- "ur5e_with_robotiq_hande_FIXED_PATHS.urdf"
- )
- urdf_path_absolute = os.path.abspath(urdf_path_relative)
- mesh_dir = files("smc")
- mesh_dir = mesh_dir.joinpath("robots")
- mesh_dir_absolute = os.path.abspath(mesh_dir)
-
- shoulder_trans = np.array([0, 0, 0.1625134425523304])
- shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
- shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy), shoulder_trans)
-
- upper_arm_trans = np.array([0.000300915150907851, 0, 0])
- upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
- upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy), upper_arm_trans)
-
- forearm_trans = np.array([-0.4249536100418752, 0, 0])
- forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
- forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy), forearm_trans)
-
- wrist_1_trans = np.array(
- [-0.3922353894477613, -0.001171506236920081, 0.1337997346972175]
- )
- wrist_1_rpy = np.array(
- [0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06]
- )
- wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy), wrist_1_trans)
-
- wrist_2_trans = np.array(
- [5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632]
- )
- wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
- wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy), wrist_2_trans)
-
- wrist_3_trans = np.array(
- [9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612]
- )
- 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)
-
- 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
- )
-
- # for whatever reason the hand-e files don't have/
- # meshcat can't read scaling information.
- # so we scale manually,
- # and the stupid gripper is in milimeters
- for geom in visual_model.geometryObjects:
- if "hand" in geom.name:
- s = geom.meshScale
- # this looks exactly correct lmao
- s *= 0.001
- geom.meshScale = s
- for geom in collision_model.geometryObjects:
- if "hand" in geom.name:
- s = geom.meshScale
- # this looks exactly correct lmao
- s *= 0.001
- geom.meshScale = s
-
- # updating joint placements.
- model.jointPlacements[1] = shoulder_se3
- model.jointPlacements[2] = upper_arm_se3
- model.jointPlacements[3] = forearm_se3
- model.jointPlacements[4] = wrist_1_se3
- model.jointPlacements[5] = wrist_2_se3
- model.jointPlacements[6] = wrist_3_se3
- # TODO: fix where the fingers end up by setting a better position here (or maybe not here idk)
- # model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
- data = pin.Data(model)
-
- return model, collision_model, visual_model, data
-
-
-def getGripperlessUR5e():
- robot = example_robot_data.load("ur5")
-
- shoulder_trans = np.array([0, 0, 0.1625134425523304])
- shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
- shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy), shoulder_trans)
-
- upper_arm_trans = np.array([0.000300915150907851, 0, 0])
- upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
- upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy), upper_arm_trans)
-
- forearm_trans = np.array([-0.4249536100418752, 0, 0])
- forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
- forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy), forearm_trans)
-
- wrist_1_trans = np.array(
- [-0.3922353894477613, -0.001171506236920081, 0.1337997346972175]
- )
- wrist_1_rpy = np.array(
- [0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06]
- )
- wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy), wrist_1_trans)
-
- wrist_2_trans = np.array(
- [5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632]
- )
- wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
- wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy), wrist_2_trans)
-
- wrist_3_trans = np.array(
- [9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612]
- )
- 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)
-
- robot.model.jointPlacements[1] = shoulder_se3
- robot.model.jointPlacements[2] = upper_arm_se3
- robot.model.jointPlacements[3] = forearm_se3
- robot.model.jointPlacements[4] = wrist_1_se3
- robot.model.jointPlacements[5] = wrist_2_se3
- robot.model.jointPlacements[6] = wrist_3_se3
- data = pin.Data(robot.model)
- return robot.model, robot.collision_model, robot.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('smc.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('smc')
- # 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 get_yumi_model():
-
- urdf_path_relative = files("smc.robots.robot_descriptions").joinpath("yumi.urdf")
- urdf_path_absolute = os.path.abspath(urdf_path_relative)
- # mesh_dir = files('smc')
- # mesh_dir_absolute = os.path.abspath(mesh_dir)
- # mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
- mesh_dir_absolute = 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_arms = pin.buildModelFromUrdf(urdf_path_absolute)
- visual_model_arms = pin.buildGeomFromUrdf(
- model_arms, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute
- )
- collision_model_arms = pin.buildGeomFromUrdf(
- model_arms,
- urdf_path_absolute,
- pin.GeometryType.COLLISION,
- None,
- mesh_dir_absolute,
- )
-
- data_arms = pin.Data(model_arms)
-
- # 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
- )
- # we should immediately set velocity limits.
- # there are no position limit by default and that is what we want.
- model_mobile_base.velocityLimit[0] = 2
- model_mobile_base.velocityLimit[1] = 2
- model_mobile_base.velocityLimit[2] = 2
- model_mobile_base.effortLimit[0] = 200
- model_mobile_base.effortLimit[1] = 2
- model_mobile_base.effortLimit[2] = 200
-
- # 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, 0.1, 0.1, 1.0])
- geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
-
- # have to add the frame manually
- model_mobile_base.addFrame(
- pin.Frame(
- "mobile_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_arms,
- geom_model_mobile_base,
- visual_model_arms,
- 1,
- pin.SE3.Identity(),
- )
- data = model.createData()
-
- return model, visual_model.copy(), 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
- # TODO TODO TODO TODO TODO TODO TODO TODO
- # TODO: heron is actually a differential drive,
- # meaning that it is not a planar joint.
- # we could put in a prismatic + revolute joint
- # as the base (both joints being at same position),
- # and that should work for our purposes.
- # this makes sense for initial testing
- # because mobile yumi's base is a planar joint
- MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
- parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
- )
- # we should immediately set velocity limits.
- # there are no position limit by default and that is what we want.
- # TODO: put in heron's values
- # TODO: make these parameters the same as in mpc_params in the planner
- model_mobile_base.velocityLimit[0] = 2
- # TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
- model_mobile_base.velocityLimit[1] = 0
- # model_mobile_base.velocityLimit[1] = 2
- model_mobile_base.velocityLimit[2] = 2
- # TODO: i have literally no idea what reasonable numbers are here
- model_mobile_base.effortLimit[0] = 200
- # TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
- model_mobile_base.effortLimit[1] = 0
- # model_mobile_base.effortLimit[1] = 2
- model_mobile_base.effortLimit[2] = 200
- # 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, 0.1, 0.1, 1.0])
- geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
-
- # have to add the frame manually
- model_mobile_base.addFrame(
- pin.Frame(
- "mobile_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
-
-
-def mir_approximation():
- # 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
- # TODO TODO TODO TODO TODO TODO TODO TODO
- # TODO: heron is actually a differential drive,
- # meaning that it is not a planar joint.
- # we could put in a prismatic + revolute joint
- # as the base (both joints being at same position),
- # and that should work for our purposes.
- # this makes sense for initial testing
- # because mobile yumi's base is a planar joint
- MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
- parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
- )
- # we should immediately set velocity limits.
- # there are no position limit by default and that is what we want.
- # TODO: put in heron's values
- # TODO: make these parameters the same as in mpc_params in the planner
- model_mobile_base.velocityLimit[0] = 2
- model_mobile_base.velocityLimit[1] = 0
- model_mobile_base.velocityLimit[2] = 2
- # TODO: i have literally no idea what reasonable numbers are here
- model_mobile_base.effortLimit[0] = 200
- model_mobile_base.effortLimit[1] = 0
- model_mobile_base.effortLimit[2] = 200
- # 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, 0.1, 0.1, 1.0])
- geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
-
- # have to add the frame manually
- # it's tool0 because that's used everywhere
- model_mobile_base.addFrame(
- pin.Frame(
- "tool0",
- MOBILE_BASE_JOINT_ID,
- 0,
- joint_placement.copy(),
- pin.FrameType.JOINT,
- )
- )
-
- data = model_mobile_base.createData()
-
- return (
- model_mobile_base,
- geom_model_mobile_base.copy(),
- geom_model_mobile_base.copy(),
- data,
- )
-
-
-# TODO:
-# try building the mobile base as a rotational joint,
-# on top of which is a prismatic joint.
-# this way you get the differential-drive basis that you want,
-# and you don't have to manually correct the additional
-# degree of freedom everywhere (jacobian, control etc)
-def heron_approximationDD():
- # 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()
- revolute_joint_name = "mobile_base_rotational_joint"
- prismatic_joint_name = "mobile_base_prismatic_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_REVOLUTE_ID = model_mobile_base.addJoint(
- parent_id, pin.JointModelRZ(), joint_placement.copy(), revolute_joint_name
- )
- MOBILE_BASE_PRISMATIC_ID = model_mobile_base.addJoint(
- MOBILE_BASE_REVOLUTE_ID,
- pin.JointModelPY(),
- joint_placement.copy(),
- prismatic_joint_name,
- )
- # we should immediately set velocity limits.
- # there are no position limit by default and that is what we want.
- # TODO: put in heron's values
- model_mobile_base.velocityLimit[0] = 2
- model_mobile_base.velocityLimit[1] = 2
- # model_mobile_base.velocityLimit[2] = 2
- # TODO: i have literally no idea what reasonable numbers are here
- model_mobile_base.effortLimit[0] = 200
- model_mobile_base.effortLimit[1] = 200
- # model_mobile_base.effortLimit[2] = 200
- # 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_PRISMATIC_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_PRISMATIC_ID, box_shape, body_placement.copy()
- )
-
- geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
- geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
-
- # have to add the frame manually
- model_mobile_base.addFrame(
- pin.Frame(
- "base",
- MOBILE_BASE_PRISMATIC_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/smc/robots/grippers/robotiq/robotiq_gripper.py b/python/smc/robots/grippers/robotiq/robotiq_gripper.py
index 977db0c09042be89a0a6c348b36dcdcbcc33d2fb..2ce85ba39eeebc6c48d5391a89c5de3481cc3163 100644
--- a/python/smc/robots/grippers/robotiq/robotiq_gripper.py
+++ b/python/smc/robots/grippers/robotiq/robotiq_gripper.py
@@ -5,7 +5,7 @@ import threading
import time
from enum import Enum
from typing import Union, Tuple, OrderedDict
-from ur_simple_control.util.grippers.abstract_gripper import AbstractGripper
+from smc.robots.grippers.abstract_gripper import AbstractGripper
class RobotiqGripper(AbstractGripper):
diff --git a/python/smc/robots/implementations/__init__.py b/python/smc/robots/implementations/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..5a211f16a31d2b89bb87e2311a8577bf58149632
--- /dev/null
+++ b/python/smc/robots/implementations/__init__.py
@@ -0,0 +1,5 @@
+from .heron import *
+from .mir import *
+from .mobile_yumi import *
+from .simulated_robot import *
+from .ur5e import *
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 67b3223d7a4bd2ebe21cd62ba6d3e33153b5c427..025c50f20b7de7faaea4fa0b0c97418a7ed34732 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -1,17 +1,37 @@
import time
-from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+from smc.robots.robotmanager_abstract import AbstractRobotManager
-class RobotManagerHeron(RobotManagerAbstract):
+class RobotManagerHeron(AbstractRobotManager):
+ @property
+ def args(self):
+ return self._args
+
+ @property
+ def model(self):
+ return self._model
+
+ @property
+ def data(self):
+ return self._data
+
+ @property
+ def visual_model(self):
+ return self._visual_model
+
+ @property
+ def collision_model(self):
+ return self._collision_model
+
def __init__(self, args):
- self.args = args
- self.model, self.collision_model, self.visual_model, self.data = (
+ self._args = args
+ self._model, self._collision_model, self._visual_model, self._data = (
heron_approximation()
)
self.ee_frame_id = self.model.getFrameId("tool0")
-class RobotManagerHeronReal(RobotManagerHeron):
+class RealHeronRobotManager(RobotManagerHeron):
def __init__(self, args):
super().__init__(args)
@@ -60,3 +80,130 @@ class RobotManagerHeronReal(RobotManagerHeron):
def unSetFreedrive(self):
# NOTE: only works for the arm
self._rtde_control.endFreedriveMode()
+
+
+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
+ # TODO TODO TODO TODO TODO TODO TODO TODO
+ # TODO: heron is actually a differential drive,
+ # meaning that it is not a planar joint.
+ # we could put in a prismatic + revolute joint
+ # as the base (both joints being at same position),
+ # and that should work for our purposes.
+ # this makes sense for initial testing
+ # because mobile yumi's base is a planar joint
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
+ parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
+ )
+ # we should immediately set velocity limits.
+ # there are no position limit by default and that is what we want.
+ # TODO: put in heron's values
+ # TODO: make these parameters the same as in mpc_params in the planner
+ model_mobile_base.velocityLimit[0] = 2
+ # TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
+ model_mobile_base.velocityLimit[1] = 0
+ # model_mobile_base.velocityLimit[1] = 2
+ model_mobile_base.velocityLimit[2] = 2
+ # TODO: i have literally no idea what reasonable numbers are here
+ model_mobile_base.effortLimit[0] = 200
+ # TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
+ model_mobile_base.effortLimit[1] = 0
+ # model_mobile_base.effortLimit[1] = 2
+ model_mobile_base.effortLimit[2] = 200
+ # 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, 0.1, 0.1, 1.0])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "mobile_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
+
+
+# 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.
+# NOTE: NOT USED, BUT STUFF WILL NEED TO BE EXTRACTED FROM THIS EVENTUALLY
+def get_heron_model_from_full_urdf():
+
+ # urdf_path_relative = files('smc.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('smc')
+ # 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
diff --git a/python/smc/robots/implementations/mir.py b/python/smc/robots/implementations/mir.py
index ae5a9c394a2ef89b3537175c5c1deacd33950493..58944c19e2d7807b307b10730439709b322bf167 100644
--- a/python/smc/robots/implementations/mir.py
+++ b/python/smc/robots/implementations/mir.py
@@ -1,4 +1,12 @@
- sendVelocityCommand(self, v):
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+
+
+class MirRobotManager(AbstractRobotManager):
+ pass
+
+
+class RealMirRobotManager(MirRobotManager):
+ def sendVelocityCommand(self, v):
"""
sendVelocityCommand
-----
@@ -18,8 +26,6 @@
# it's not correct, but it's more correct than not updating
# self.q = pin.integrate(self.model, self.q, qd * self.dt)
-
-
def stopRobot(self):
print("stopping via freedrive lel")
self._rtde_control.freedriveMode()
@@ -33,3 +39,79 @@
def unSetFreedrive(self):
self._rtde_control.endFreedriveMode()
raise NotImplementedError("freedrive function only written for ur5e")
+
+
+def mir_approximation():
+ # 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
+ # TODO TODO TODO TODO TODO TODO TODO TODO
+ # TODO: heron is actually a differential drive,
+ # meaning that it is not a planar joint.
+ # we could put in a prismatic + revolute joint
+ # as the base (both joints being at same position),
+ # and that should work for our purposes.
+ # this makes sense for initial testing
+ # because mobile yumi's base is a planar joint
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
+ parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
+ )
+ # we should immediately set velocity limits.
+ # there are no position limit by default and that is what we want.
+ # TODO: put in heron's values
+ # TODO: make these parameters the same as in mpc_params in the planner
+ model_mobile_base.velocityLimit[0] = 2
+ model_mobile_base.velocityLimit[1] = 0
+ model_mobile_base.velocityLimit[2] = 2
+ # TODO: i have literally no idea what reasonable numbers are here
+ model_mobile_base.effortLimit[0] = 200
+ model_mobile_base.effortLimit[1] = 0
+ model_mobile_base.effortLimit[2] = 200
+ # 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, 0.1, 0.1, 1.0])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ # it's tool0 because that's used everywhere
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "tool0",
+ MOBILE_BASE_JOINT_ID,
+ 0,
+ joint_placement.copy(),
+ pin.FrameType.JOINT,
+ )
+ )
+
+ data = model_mobile_base.createData()
+
+ return (
+ model_mobile_base,
+ geom_model_mobile_base.copy(),
+ geom_model_mobile_base.copy(),
+ data,
+ )
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 7cf3176e3349d9cda9d65cc86c5beeaa49305085..1842c64bf561760e91401d87f67d809ed5760dcc 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -1,7 +1,8 @@
-from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
-class RobotManagerMobileYumi(RobotManagerAbstract, DualArmInterface):
+class MobileYumiRobotManager(DualArmInterface):
def __init__(self, args):
self.args = args
self.model, self.collision_model, self.visual_model, self.data = (
@@ -11,7 +12,7 @@ class RobotManagerMobileYumi(RobotManagerAbstract, DualArmInterface):
self.l_ee_frame_id = self.model.getFrameId("robl_joint_7")
-class RobotManagerMobileYumiReal(RobotManagerMobileYumi):
+class RealMobileYumiRobotManager(MobileYumiRobotManager):
def __init__(self, args):
super().__init__(args)
@@ -39,3 +40,96 @@ class RobotManagerMobileYumiReal(RobotManagerMobileYumi):
msg.velocity[i] = qd_cmd[i - 12]
self.publisher_joints_cmd.publish(msg)
+
+
+def get_yumi_model():
+
+ urdf_path_relative = files("smc.robots.robot_descriptions").joinpath("yumi.urdf")
+ urdf_path_absolute = os.path.abspath(urdf_path_relative)
+ # mesh_dir = files('smc')
+ # mesh_dir_absolute = os.path.abspath(mesh_dir)
+ # mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
+ mesh_dir_absolute = 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_arms = pin.buildModelFromUrdf(urdf_path_absolute)
+ visual_model_arms = pin.buildGeomFromUrdf(
+ model_arms, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute
+ )
+ collision_model_arms = pin.buildGeomFromUrdf(
+ model_arms,
+ urdf_path_absolute,
+ pin.GeometryType.COLLISION,
+ None,
+ mesh_dir_absolute,
+ )
+
+ data_arms = pin.Data(model_arms)
+
+ # 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
+ )
+ # we should immediately set velocity limits.
+ # there are no position limit by default and that is what we want.
+ model_mobile_base.velocityLimit[0] = 2
+ model_mobile_base.velocityLimit[1] = 2
+ model_mobile_base.velocityLimit[2] = 2
+ model_mobile_base.effortLimit[0] = 200
+ model_mobile_base.effortLimit[1] = 2
+ model_mobile_base.effortLimit[2] = 200
+
+ # 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, 0.1, 0.1, 1.0])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "mobile_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_arms,
+ geom_model_mobile_base,
+ visual_model_arms,
+ 1,
+ pin.SE3.Identity(),
+ )
+ data = model.createData()
+
+ return model, visual_model.copy(), visual_model, data
diff --git a/python/smc/robots/implementations/simulated_robot.py b/python/smc/robots/implementations/simulated_robot.py
index 43bcb015976f59af1c78f9b95d9551f97e636b19..45091b32dd9a6c2e0223e585338ebea66d425402 100644
--- a/python/smc/robots/implementations/simulated_robot.py
+++ b/python/smc/robots/implementations/simulated_robot.py
@@ -1,13 +1,13 @@
-from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
-from ur_simple_control.util.get_model import *
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+import pinocchio as pin
-class RobotManagerSimulated(RobotManagerAbstract):
+class SimulatedRobotManager(AbstractRobotManager):
# TODO: find a way for this to inherit from the particular robot manager (the non-real part)
def __init__(self, args):
pass
- def sendVelocityCommand(self, v):
+ def sendVelocityCommandToReal(self, v):
"""
sendVelocityCommand
-----
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index 2f6404c9dfc8bde73097151e7dccbc9bdafffeb7..d1044ca24ea99034a5bd276916c890fedff754fb 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -1,32 +1,54 @@
-from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.robots.interfaces.force_torque_sensor_interface import (
- ForceTorqueSensorInterface,
+ ForceTorqueOnSingleArmWrist,
)
-from smc.util.get_model import get_model
+from smc.robots.grippers.robotiq.robotiq_gripper import RobotiqGripper
+from smc.robots.grippers.on_robot.twofg import TwoFG
+
import numpy as np
import pinocchio as pin
-
-# TODO: make rtde optional - ofc assert it has to exist in UR5e real constructor
+from importlib.resources import files
+import time
+import argparse
+from os import path
from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
from rtde_io import RTDEIOInterface
-from smc.util.grippers.robotiq.robotiq_gripper import RobotiqGripper
-from smc.util.grippers.on_robot.twofg import TWOFG
-import time
+
+from smc.robots.robotmanager_abstract import AbstractRealRobotManager
# NOTE: this one assumes a jaw gripper
-class RobotManagerUR5e(SingleArmInterface, ForceTorqueSensorInterface):
+class RobotManagerUR5e(ForceTorqueOnSingleArmWrist):
+
+ @property
+ def model(self):
+ return self._model
+
+ @property
+ def data(self):
+ return self._data
+
+ @property
+ def visual_model(self):
+ return self._visual_model
+
+ @property
+ def collision_model(self):
+ return self._collision_model
+
def __init__(self, args):
- self.args = args
- self.model, self.collision_model, self.visual_model, self.data = get_model()
+ if args.debug_prints:
+ print("RobotManagerUR5e init")
+ self._model, self._collision_model, self._visual_model, self._data = get_model()
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
+ super().__init__(args)
-class RobotManagerU5eReal(RobotManagerUR5e):
- def __init__(self, args):
- super().__init__(args)
+class RealUR5eRobotManager(RobotManagerUR5e, AbstractRealRobotManager):
+ def __init__(self, args: argparse.Namespace):
+ if args.debug_prints:
+ print("RealUR5eRobotManager init")
# NOTE: UR's sleep slider is evil and nothing works unless if it's set to 1.0!!!
# you have to control the acceleration via the acceleration argument.
# we need to set it to 1.0 with ur_rtde so that's why it's here and explicitely named
@@ -34,33 +56,31 @@ class RobotManagerU5eReal(RobotManagerUR5e):
self._rtde_control: RTDEControlInterface
self._rtde_receive: RTDEReceiveInterface
self._rtde_io: RTDEIOInterface
- self.connectToRobot()
- self.connectToGripper()
- self.setInitialPose()
+ super().__init__(args)
def connectToGripper(self):
- if (self.args.gripper != "none") and self.args.real:
- if self.args.gripper == "robotiq":
- self.gripper = RobotiqGripper()
- self.gripper.connect(self.args.robot_ip, 63352)
- self.gripper.activate()
- if self.args.gripper == "onrobot":
- self.gripper = TWOFG()
- if self.args.gripper == "none":
+ if (self.args.gripper == "none") or not self.args.real:
self.gripper = None
+ return
+ if self.args.gripper == "robotiq":
+ self.gripper = RobotiqGripper()
+ self.gripper.connect(self.args.robot_ip, 63352)
+ self.gripper.activate()
+ if self.args.gripper == "onrobot":
+ self.gripper = TwoFG()
def setInitialPose(self):
if not self.args.real and self.args.start_from_current_pose:
self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
- self.q = np.array(self._rtde_receive.getActualQ())
+ self._q = np.array(self._rtde_receive.getActualQ())
if self.args.visualize_manipulator:
- self.visualizer_manager.sendCommand({"q": self.q})
+ self.visualizer_manager.sendCommand({"q": self._q})
if not self.args.real and not self.args.start_from_current_pose:
- self.q = pin.randomConfiguration(
+ self._q = pin.randomConfiguration(
self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
)
if self.args.real:
- self.q = np.array(self._rtde_receive.getActualQ())
+ self._q = np.array(self._rtde_receive.getActualQ())
def connectToRobot(self):
if self.args.real:
@@ -76,7 +96,7 @@ class RobotManagerU5eReal(RobotManagerUR5e):
# NOTE: the force/torque sensor just has large offsets for no reason,
# and you need to minus them to have usable readings.
# we provide this with calibrateFT
- self.wrench_offset = self.calibrateFT()
+ self.wrench_offset = self.calibrateFT(self._dt)
else:
self.wrench_offset = np.zeros(6)
@@ -91,31 +111,32 @@ class RobotManagerU5eReal(RobotManagerUR5e):
self._rtde_io.setSpeedSlider(value)
self.speed_slider = value
- def _getQ(self):
+ def _updateQ(self):
q = self._rtde_receive.getActualQ()
self._q = np.array(q)
- def _getV(self):
+ def _updateV(self):
v = self._rtde_receive.getActualQd()
self._v = np.array(v)
- def _getWrench(self):
- # NOTE: UR5e's ft-sensors gives readings in robot's base frame
+ def _updateWrench(self):
if not self.args.real:
+ self._wrench_base = np.random.random(6)
+ else:
+ # NOTE: UR5e's ft-sensors gives readings in robot's base frame
self._wrench_base = (
np.array(self._rtde_receive.getActualTCPForce()) - self._wrench_bias
)
- else:
- # TODO: do something better here (at least a better distribution)
- self._wrench_base = np.random.random(6)
# NOTE: we define the default wrench to be given in the end-effector frame
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_base
- def sendVelocityCommand(self, v):
- jdflkdahfjkdlsa nkjdf;jnsao
+ def zeroFtSensor(self):
+ self._rtde_control.zeroFtSensor()
+
+ def sendVelocityCommandToReal(self, v):
# speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
self._rtde_control.speedJ(v, self._acceleration, self._dt)
@@ -132,3 +153,137 @@ class RobotManagerU5eReal(RobotManagerUR5e):
def unSetFreedrive(self):
self._rtde_control.endFreedriveMode()
raise NotImplementedError("freedrive function only written for ur5e")
+
+
+def get_model():
+
+ urdf_path_relative = files("smc.robots.robot_descriptions.urdf").joinpath(
+ "ur5e_with_robotiq_hande_FIXED_PATHS.urdf"
+ )
+ urdf_path_absolute = path.abspath(urdf_path_relative)
+ mesh_dir = files("smc")
+ mesh_dir = mesh_dir.joinpath("robots")
+ mesh_dir_absolute = path.abspath(mesh_dir)
+
+ shoulder_trans = np.array([0, 0, 0.1625134425523304])
+ shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
+ shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy), shoulder_trans)
+
+ upper_arm_trans = np.array([0.000300915150907851, 0, 0])
+ upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
+ upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy), upper_arm_trans)
+
+ forearm_trans = np.array([-0.4249536100418752, 0, 0])
+ forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
+ forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy), forearm_trans)
+
+ wrist_1_trans = np.array(
+ [-0.3922353894477613, -0.001171506236920081, 0.1337997346972175]
+ )
+ wrist_1_rpy = np.array(
+ [0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06]
+ )
+ wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy), wrist_1_trans)
+
+ wrist_2_trans = np.array(
+ [5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632]
+ )
+ wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
+ wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy), wrist_2_trans)
+
+ wrist_3_trans = np.array(
+ [9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612]
+ )
+ 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)
+
+ 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
+ )
+
+ # for whatever reason the hand-e files don't have/
+ # meshcat can't read scaling information.
+ # so we scale manually,
+ # and the stupid gripper is in milimeters
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+ for geom in collision_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+
+ # updating joint placements.
+ model.jointPlacements[1] = shoulder_se3
+ model.jointPlacements[2] = upper_arm_se3
+ model.jointPlacements[3] = forearm_se3
+ model.jointPlacements[4] = wrist_1_se3
+ model.jointPlacements[5] = wrist_2_se3
+ model.jointPlacements[6] = wrist_3_se3
+ # TODO: fix where the fingers end up by setting a better position here (or maybe not here idk)
+ # model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
+ data = pin.Data(model)
+
+ return model, collision_model, visual_model, data
+
+
+def getGripperlessUR5e():
+ import example_robot_data
+
+ robot = example_robot_data.load("ur5")
+
+ shoulder_trans = np.array([0, 0, 0.1625134425523304])
+ shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
+ shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy), shoulder_trans)
+
+ upper_arm_trans = np.array([0.000300915150907851, 0, 0])
+ upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
+ upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy), upper_arm_trans)
+
+ forearm_trans = np.array([-0.4249536100418752, 0, 0])
+ forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
+ forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy), forearm_trans)
+
+ wrist_1_trans = np.array(
+ [-0.3922353894477613, -0.001171506236920081, 0.1337997346972175]
+ )
+ wrist_1_rpy = np.array(
+ [0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06]
+ )
+ wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy), wrist_1_trans)
+
+ wrist_2_trans = np.array(
+ [5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632]
+ )
+ wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
+ wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy), wrist_2_trans)
+
+ wrist_3_trans = np.array(
+ [9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612]
+ )
+ 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)
+
+ robot.model.jointPlacements[1] = shoulder_se3
+ robot.model.jointPlacements[2] = upper_arm_se3
+ robot.model.jointPlacements[3] = forearm_se3
+ robot.model.jointPlacements[4] = wrist_1_se3
+ robot.model.jointPlacements[5] = wrist_2_se3
+ robot.model.jointPlacements[6] = wrist_3_se3
+ data = pin.Data(robot.model)
+ return robot.model, robot.collision_model, robot.visual_model, data
diff --git a/python/smc/robots/interfaces/force_torque_sensor_interface.py b/python/smc/robots/interfaces/force_torque_sensor_interface.py
index 268e9485abf8fa3f7a9c223e880bcb8d87df2693..39800d29e20947bf0efc8b78bdcc2645770383d3 100644
--- a/python/smc/robots/interfaces/force_torque_sensor_interface.py
+++ b/python/smc/robots/interfaces/force_torque_sensor_interface.py
@@ -1,3 +1,5 @@
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
import abc
import numpy as np
import time
@@ -6,10 +8,11 @@ import time
class ForceTorqueSensorInterface(abc.ABC):
def __init__(self):
# in base frame
- self._wrench_base = np.zeros(6)
- self._wrench = np.zeros(6)
+ print("ForceTorqueSensorInterface init")
+ self._wrench_base: np.ndarray = np.zeros(6)
+ self._wrench: np.ndarray = np.zeros(6)
# NOTE: wrench bias will be defined in the frame your sensor's gives readings
- self._wrench_bias = np.zeros(6)
+ self._wrench_bias: np.ndarray = np.zeros(6)
# get this via model.getFrameId("frame_name")
@property
@@ -38,26 +41,26 @@ class ForceTorqueSensorInterface(abc.ABC):
return self._wrench_base.copy()
@abc.abstractmethod
- def getWrenchFromReal(self):
+ def _updateWrench(self):
"""
get wrench reading from whatever interface you have.
NOTE:
- 1) it is YOUR job to know in which frame the reading is, and to map this
- BOTH to the base frame of the robot AND your prefered frame.
- this is because it is impossible to know what's the default on your sensor or where it is.
- this way you are forced to verify and know in which frame you get your readings from.
- 2) do NOT do any filtering in this method - that happens elsewhere - here we just want the reading
- in correct frames
- 3) you can count on your RobotManager to have already called forward kinematics before this function, i.e.
- you can use frames stored in your RobotManager's attributes
- 4) if self.args.real == False, provide noise for better simulation testing
+ 1) it is YOUR job to know in which frame the reading is, and to map this
+ BOTH to the base frame of the robot AND your prefered frame.
+ this is because it is impossible to know what's the default on your sensor or where it is.
+ this way you are forced to verify and know in which frame you get your readings from.
+ 2) do NOT do any filtering in this method - that happens elsewhere -
+ here we just want the reading in correct frames
+ 3) you HAVE TO include this function in an overriden _step function
+ 4) if self.args.real == False, provide noise for better simulation testing
"""
pass
- def _getWrenchInEE(self):
- raise NotImplementedError
+ @abc.abstractmethod
+ def zeroFtSensor(self) -> None:
+ pass
- def calibrateFT(self, robot):
+ def calibrateFT(self, dt):
"""
calibrateFT
-----------
@@ -79,17 +82,27 @@ class ForceTorqueSensorInterface(abc.ABC):
# 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!!!
- robot.rtde_control.zeroFtSensor()
- for i in range(2000):
+ self.zeroFtSensor()
+ for _ in range(2000):
start = time.time()
- ft = robot.rtde_receive.getActualTCPForce()
+ ft = self._updateWrench()
ft_readings.append(ft)
end = time.time()
diff = end - start
- if diff < self.dt:
- time.sleep(self.dt - diff)
+ if diff < dt:
+ time.sleep(dt - diff)
ft_readings = np.array(ft_readings)
- self.wrench_offset = np.average(ft_readings, axis=0)
- print(self.wrench_offset)
- return self.wrench_offset.copy()
+ self._wrench_bias = np.average(ft_readings, axis=0)
+ print("The wrench bias is:", self._wrench_bias)
+ return self._wrench_bias.copy()
+
+
+class ForceTorqueOnSingleArmWrist(SingleArmInterface, ForceTorqueSensorInterface):
+ @property
+ def ft_sensor_frame_id(self) -> int:
+ return self._ee_frame_id
+
+ def _step(self):
+ super()._step()
+ self._updateWrench()
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index b3c09792c01ffd70487abe1bb111ed31cd5738bd..4cc7cf386a38118146f111fd1853777f99fb0e47 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -1,34 +1,42 @@
-import abs
import numpy as np
import pinocchio as pin
-from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+from smc.robots.robotmanager_abstract import AbstractRobotManager
-class SingleArmInterface(RobotManagerAbstract):
- def __init__(self):
- self._ee_frame_id = self.model.getFrameId("tool0")
+class SingleArmInterface(AbstractRobotManager):
+ def __init__(self, args):
+ if args.debug_prints:
+ print("SingleArmInterface init")
+ self._ee_frame_id: int = self.model.getFrameId("tool0")
self._T_w_e: pin.SE3
+ super().__init__(args)
- def getT_w_e(self, q=None):
- if q is None:
- return self._T_w_e.copy()
+ @property
+ def T_w_e(self):
+ return self._T_w_e.copy()
+
+ def computeT_w_e(self, q) -> pin.SE3:
assert type(q) is np.ndarray
- # calling these here is ok because we rely
- # on robotmanager attributes instead of model.something
- # (which is copying data, but fully separates state and computation,
- # which is important in situations like this)
pin.forwardKinematics(
self.model,
self.data,
q,
- # np.zeros(self.model.nv),
- # np.zeros(self.model.nv),
)
- # NOTE: this also returns the frame, so less copying possible
+ # alternative if you want all frames
# pin.updateFramePlacements(self.model, self.data)
pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
return self.data.oMf[self._ee_frame_id].copy()
def forwardKinematics(self):
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ self._q,
+ )
pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
self._T_w_e = self.data.oMf[self._ee_frame_id].copy()
+
+ def _step(self):
+ self._updateQ()
+ self._updateV()
+ self.forwardKinematics()
diff --git a/python/smc/robots/notes_on_getting_model_from_urdf.md b/python/smc/robots/notes_on_getting_model_from_urdf.md
new file mode 100644
index 0000000000000000000000000000000000000000..0aa899a3191201c7b042db8f45cd67554bf0fda5
--- /dev/null
+++ b/python/smc/robots/notes_on_getting_model_from_urdf.md
@@ -0,0 +1,28 @@
+"""
+possible improvement:
+ get the calibration file of the robot without ros
+ and then put it here.
+ these magic numbers are not a good look.
+"""
+
+# can't get the urdf reading with these functions to save my life, idk what or why
+
+#############################################################
+# PACKAGE_DIR IS THE WHOLE smc 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.
+#############################################################
+
+"""
+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
+"""
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 28d4c8692dd1605d721b16f8c99be509d5fa71d3..cb8409f613decf1ddc29df901307cd60198897ea 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -1,5 +1,5 @@
-from smc.visualize.visualize import manipulatorVisualizer
-from smc.util.logging_utils import LogManager
+from smc.visualization.visualizer import manipulatorVisualizer
+from smc.logging.logger import Logger
from smc.multiprocessing.process_manager import ProcessManager
import abc
import argparse
@@ -15,8 +15,8 @@ class AbstractRobotManager(abc.ABC):
- design goal: rely on pinocchio as much as possible while
concealing obvious bookkeeping
- this class serves to:
- - store the non-interfacing information
- about the robot like maximum allowed velocity, pin.model etc
+ - store the non-interfacing information about the robot
+ like maximum allowed velocity, pin.model etc
- provide functions every robot has to have like getQ
"""
@@ -37,11 +37,6 @@ class AbstractRobotManager(abc.ABC):
# then you will get a TypeError, and that's what we want -
# errors if you didn't implement these attributes.
- # NOTE: these arguments you need to implement yourself
- @property
- @abc.abstractmethod
- def args(self) -> argparse.Namespace: ...
-
@property
@abc.abstractmethod
def model(self) -> pin.Model: ...
@@ -56,8 +51,10 @@ class AbstractRobotManager(abc.ABC):
@abc.abstractmethod
def collision_model(self) -> pin.pinocchio_pywrap_default.GeometryModel: ...
- # NOTE: call this via super().__init__() after you've implemented the above abstract properties
- def __init__(self, args):
+ def __init__(self, args: argparse.Namespace):
+ self.args = args
+ if args.debug_prints:
+ print("AbstractRobotManager init")
####################################################################
# robot-related attributes #
####################################################################
@@ -77,7 +74,7 @@ class AbstractRobotManager(abc.ABC):
# _MAX_V is the robot's hardware joint velocity limit
self._MAX_V: np.ndarray = self.model.velocityLimit
# but you usually want to run at lower speeds for safety reasons
- assert self.args.max_v_percentage <= 1.0 and self.args.max_v_pecentage > 0.0
+ assert self.args.max_v_percentage <= 1.0 and self.args.max_v_percentage > 0.0
# so velocity commands are internally clipped to self._max_v
self._max_v = np.clip(self._MAX_V, 0.0, args.max_v_percentage * self._MAX_V)
# NOTE: make sure you've read pinocchio docs and understand why
@@ -89,18 +86,16 @@ class AbstractRobotManager(abc.ABC):
# TODO: each robot should know which grippers are available
# and set this there.
self.gripper = None
- self._step()
####################################################################
# processes and utilities robotmanager owns #
####################################################################
# TODO: make a default
if args.save_log:
- self._log_manager = LogManager(args)
+ self._log_manager = Logger(args)
# since there is only one robot and one visualizer, this is robotmanager's job
- self.visualizer_manager: None | ProcessManager
- self.visualizer_manager = None
+ self.visualizer_manager: ProcessManager
# TODO: this should be transferred to an multiprocess manager
if self.args.visualizer:
side_function = partial(
@@ -134,6 +129,10 @@ class AbstractRobotManager(abc.ABC):
"""
pass
+ @property
+ def dt(self):
+ return self._dt
+
@property
def q(self):
return self._q.copy()
@@ -142,6 +141,29 @@ class AbstractRobotManager(abc.ABC):
def v(self):
return self._v.copy()
+ # _updateQ and _updateV could be put into getters and setters of q and v respectively.
+ # however, because i don't trust the interfaces of a random robot to play ball,
+ # i want to keep this separated.
+ # the point of q and v as properties is that you don't have to worry about using q or v when you like,
+ # and this is not the case if a shitty interface creates lag upon every call.
+ # if these functions are separate then i can guarantee that they are called once per control cycle in step
+ # and i can know i used the shitty interface the least amount of times required - once per cycle.
+ @abc.abstractmethod
+ def _updateQ(self):
+ """
+ update internal _q joint angles variable with current angles obtained
+ from your robot's communication interface
+ """
+ pass
+
+ @abc.abstractmethod
+ def _updateV(self):
+ """
+ update internal _v joint velocities variable with current velocities obtained
+ from your robot's communication interface
+ """
+ pass
+
@abc.abstractmethod
def forwardKinematics(self):
"""
@@ -196,6 +218,47 @@ class AbstractRobotManager(abc.ABC):
@abc.abstractmethod
def sendVelocityCommandToReal(self, v): ...
+ @abc.abstractmethod
+ def stopRobot(self):
+ """
+ stopRobot
+ ---------
+ implement whatever stops the robot as cleanly as possible on your robot.
+ does not need to be emergency option, could be sending zero velocity
+ commands, could be switching to freedrive/leadthrough/whatever.
+
+ depending on the communication interface on your robot,
+ it could happen that it stays in "externally guided motion" mode,
+ i.e. it keeps listening to new velocity commands, while executing the last velocity command.
+ such a scenario is problematic because they last control command might not be
+ all zeros, meaning the robot will keep slowly moving and hit something after 5min
+ (obviously this exact thing happened to the author...)
+ """
+ pass
+
+ @abc.abstractmethod
+ def setFreedrive(self):
+ """
+ setFreedrive
+ ------------
+ set the robot into the "teaching mode", "freedrive mode", "leadthrough mode"
+ or whatever the term is for when you can manually (with your hands) move the robot
+ around
+ """
+ pass
+
+ @abc.abstractmethod
+ def unSetFreedrive(self):
+ """
+ setFreedrive
+ ------------
+ unset the robot into the "teaching mode", "freedrive mode", "leadthrough mode"
+ or whatever the term is for when you can manually (with your hands) move the robot
+ around, back to "wait" or "accept commands" or whatever mode the robot has to
+ receive external commands.
+ """
+ pass
+
# TODO: make faux gripper class to avoid this bullshit here
def openGripper(self):
if self.gripper is None:
@@ -251,7 +314,7 @@ class AbstractRobotManager(abc.ABC):
print("you didn't select viz")
-class AbstractRobotManagerReal(AbstractRobotManager, abc.ABC):
+class AbstractRealRobotManager(AbstractRobotManager, abc.ABC):
"""
RobotManagerRealAbstract
------------------------
@@ -261,6 +324,8 @@ class AbstractRobotManagerReal(AbstractRobotManager, abc.ABC):
def __init__(self, args):
super().__init__(args)
+ if args.debug_prints:
+ print("AbstractRealRobotManager init")
self.connectToRobot()
self.connectToGripper()
self.setInitialPose()
@@ -288,70 +353,3 @@ class AbstractRobotManagerReal(AbstractRobotManager, abc.ABC):
is the same, the robot is likely to have its own way of interfacing it
"""
pass
-
- @abc.abstractmethod
- def _getQ(self):
- """
- update internal _q joint angles variable with current angles obtained
- from your robot's communication interface
- """
- pass
-
- @abc.abstractmethod
- def _getV(self):
- """
- update internal _v joint velocities variable with current velocities obtained
- from your robot's communication interface
- """
- pass
-
- # @abc.abstractmethod
- # def _step(self):
- # self._getQ()
- # self._getV()
- # # self._getWrench()
- #
- # # NOTE: when implementing an interface for torque-controlled robots,
- # # use computeAllTerms to make life easier
- # self.forwardKinematics()
-
- @abc.abstractmethod
- def stopRobot(self):
- """
- stopRobot
- ---------
- implement whatever stops the robot as cleanly as possible on your robot.
- does not need to be emergency option, could be sending zero velocity
- commands, could be switching to freedrive/leadthrough/whatever.
-
- depending on the communication interface on your robot,
- it could happen that it stays in "externally guided motion" mode,
- i.e. it keeps listening to new velocity commands, while executing the last velocity command.
- such a scenario is problematic because they last control command might not be
- all zeros, meaning the robot will keep slowly moving and hit something after 5min
- (obviously this exact thing happened to the author...)
- """
- pass
-
- @abc.abstractmethod
- def setFreedrive(self):
- """
- setFreedrive
- ------------
- set the robot into the "teaching mode", "freedrive mode", "leadthrough mode"
- or whatever the term is for when you can manually (with your hands) move the robot
- around
- """
- pass
-
- @abc.abstractmethod
- def unSetFreedrive(self):
- """
- setFreedrive
- ------------
- unset the robot into the "teaching mode", "freedrive mode", "leadthrough mode"
- or whatever the term is for when you can manually (with your hands) move the robot
- around, back to "wait" or "accept commands" or whatever mode the robot has to
- receive external commands.
- """
- pass
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index 35e3cf733c192a4e735427dc991daa2d093eabcf..7a48694d4346e5d56310b24314a65d876a2019c9 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -28,9 +28,9 @@ def getMinimalArgParser():
choices=["ur5e", "heron", "heronros", "gripperlessur5e", "mirros", "yumi"],
)
parser.add_argument(
- "--simulation",
+ "--real",
action=argparse.BooleanOptionalAction,
- help="whether you are running the UR simulator",
+ help="whether you're running on the real robot or not",
default=False,
)
parser.add_argument(
@@ -39,12 +39,6 @@ def getMinimalArgParser():
help="robot's ip address (only needed if running on the real robot)",
default="192.168.1.102",
)
- parser.add_argument(
- "--pinocchio-only",
- action=argparse.BooleanOptionalAction,
- help="whether you want to just integrate with pinocchio",
- default=True,
- )
parser.add_argument(
"--ctrl-freq",
type=int,
@@ -81,14 +75,6 @@ def getMinimalArgParser():
help="maximum allowable iteration number (it runs at 500Hz)",
default=100000,
)
- parser.add_argument(
- "--speed-slider",
- type=float,
- help="cap robot's speed with the speed slider \
- to something between 0 and 1, 0.5 by default \
- BE CAREFUL WITH THIS.",
- default=1.0,
- )
parser.add_argument(
"--start-from-current-pose",
action=argparse.BooleanOptionalAction,
@@ -178,7 +164,7 @@ def getMinimalArgParser():
"--visualize-collision-approximation",
action=argparse.BooleanOptionalAction,
help="whether you want to visualize the collision approximation used in controllers with obstacle avoidance",
- default=True,
+ default=False,
)
return parser
diff --git a/python/smc/util/draw_path.py b/python/smc/util/draw_path.py
index 29cf10423471560c052ceb02704d0ffebae40ada..9e897089c370d964c75190f842b295e3a734f2d8 100644
--- a/python/smc/util/draw_path.py
+++ b/python/smc/util/draw_path.py
@@ -12,17 +12,20 @@ possible improvement: make it all bezier curves
import numpy as np
import matplotlib.pyplot as plt
+
# LassoSelector is used for drawing.
# The class actually just does the drawing with mouse input,
# and the rest of processing is done on the obtained path.
# Thus it is the correct tool for the job and there's no need
# to reimplement it from mouse events.
from matplotlib.widgets import LassoSelector
+
# Path is the most generic matplotlib class.
# It has some convenient functions to handle the draw line,
-# i.e. collection of points, but just refer to matplotlib
+# i.e. collection of points, but just refer to matplotlib
# documentation and examples on Lasso and Path to see more.
-#from matplotlib.path import Path
+# from matplotlib.path import Path
+
class DrawPathManager:
def __init__(self, args, ax):
@@ -32,15 +35,14 @@ class DrawPathManager:
def onselect(self, verts):
# verts is a list of tuples
- self.path = np.array( [ [i[0], i[1]] for i in verts ] )
+ self.path = np.array([[i[0], i[1]] for i in verts])
# this would refresh the canvas and remove the drawing
- #self.canvas.draw_idle()
+ # self.canvas.draw_idle()
def disconnect(self):
self.lasso.disconnect_events()
self.canvas.draw_idle()
-
# function we bind to key press even
# made to save and exit
def accept(self, event):
@@ -49,10 +51,11 @@ class DrawPathManager:
print("pixel path:")
print(self.path)
self.disconnect()
- np.savetxt("./path_in_pixels.csv", self.path, delimiter=',', fmt='%.5f')
+ np.savetxt("./path_in_pixels.csv", self.path, delimiter=",", fmt="%.5f")
# plt.close over exit so that we can call this from elsewhere and not kill the program
plt.close()
+
def drawPath(args):
# normalize both x and y to 0-1 range
# we can multiply however we want later
@@ -68,18 +71,21 @@ def drawPath(args):
# map key press to our function
# thankfully it didn't mind have self as the first argument
fig.canvas.mpl_connect("key_press_event", selector.accept)
- ax.set_title("The drawing has to be 1 continuous line. Press 'Enter' to accept the drawn path. ")
+ ax.set_title(
+ "The drawing has to be 1 continuous line. Press 'Enter' to accept the drawn path. "
+ )
plt.show()
return selector.path
-if __name__ == '__main__':
- args = get_args()
- drawPath(args)
- #plt.ion()
- #fig = plt.figure()
- #canvas = fig.canvas
- #ax = fig.add_subplot(111)
- #ax.plot(np.arange(100), np.sin(np.arange(100)))
- #canvas.draw()
- #canvas.flush_events()
+if __name__ == "__main__":
+ pass
+# args = get_args()
+# drawPath(args)
+# plt.ion()
+# fig = plt.figure()
+# canvas = fig.canvas
+# ax = fig.add_subplot(111)
+# ax.plot(np.arange(100), np.sin(np.arange(100)))
+# canvas.draw()
+# canvas.flush_events()
diff --git a/python/smc/util/encapsulating_ellipses.py b/python/smc/util/encapsulating_ellipses.py
index 80dd2081177d25516a243a6ecea25ef8adc89fda..ae22df6281375e46b2f7b9686b53201f88756125 100644
--- a/python/smc/util/encapsulating_ellipses.py
+++ b/python/smc/util/encapsulating_ellipses.py
@@ -25,9 +25,11 @@ import casadi
import numpy as np
import pinocchio as pin
from pinocchio import casadi as cpin
-from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
-from ur_simple_control.managers import RobotManager
-from ur_simple_control.util.get_model import get_model, get_heron_model
+from smc.visualize.meshcat_viewer_wrapper.visualizer import (
+ MeshcatVisualizer,
+)
+from smc.managers import RobotManager
+from smc.util.get_model import get_model, get_heron_model
from types import SimpleNamespace
import time
import pickle
@@ -39,7 +41,12 @@ import numpy as np
import time
import argparse
from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from smc.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
+
def plotEllipseMatplotlib(ax, opti_A, opti_c):
# find the rotation matrix and radii of the axes
@@ -59,18 +66,20 @@ def plotEllipseMatplotlib(ax, opti_A, opti_c):
ax.plot_wireframe(x, y, z, rstride=4, cstride=4, color="b", alpha=0.2)
+
# TODO: finish and verify
# make a separate function for a single ellipse based on the namespace
-def visualizeEllipses(args, robot : RobotManager, ellipses):
+def visualizeEllipses(args, robot: RobotManager, ellipses):
e, P = np.linalg.eig()
ellipse_placement = pin.SE3(P, ellipse.center)
- #ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
+ # ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
# TODO: add to MeshcatVisualizerWrapper,
# add queue handling in visualize function
viz.addEllipsoid(f"el_{ellipse.name}", sol_r, [0.3, 0.9, 0.3, 0.3])
viz.applyConfiguration(f"el_{ellipse.name}", ellipse_placement)
+
# plotting the vertices in the ellipse in matplotlib for verification
def plotVertices(ax, vertices, nsubsample):
"""
@@ -98,23 +107,26 @@ def plotVertices(ax, vertices, nsubsample):
bottom=plot_center[2] - plot_length, top=plot_center[2] + plot_length
)
-def visualizeVertices(args, robot : RobotManager):
+
+def visualizeVertices(args, robot: RobotManager):
for i, geom in enumerate(robot.collision_model.geometryObjects):
vertices = geom.geometry.vertices()
-
+
for i in np.arange(0, vertices.shape[0]):
viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
vertix_pose = pin.SE3.Identity()
vertix_pose.translation = vertices[i]
- #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
- vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
- #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
+ # vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
+ vertix_pose = data.oMf[geom.parentFrame].act(
+ geom.placement.act(vertix_pose)
+ )
+ # viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
viz.applyConfiguration(f"world/point_{i}", vertix_pose)
# TODO: make this for every robot.
-def computeEncapsulatingEllipses(args, robot : RobotManager):
+def computeEncapsulatingEllipses(args, robot: RobotManager):
"""
computeEncapsulatingEllipses
----------------------------
@@ -128,38 +140,50 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
and since this is done once per robot there's no point
to writing the algorithm (more work overall).
"""
- model, collision_model, visual_model, data = (robot.model, robot.collision_model, robot.visual_model, robot.data)
- viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
- #q = np.zeros(model.nq)
+ model, collision_model, visual_model, data = (
+ robot.model,
+ robot.collision_model,
+ robot.visual_model,
+ robot.data,
+ )
+ viz = MeshcatVisualizer(
+ model=model, collision_model=collision_model, visual_model=visual_model
+ )
+ # q = np.zeros(model.nq)
q = pin.randomConfiguration(model)
print(q)
viz.display(q)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
- pin.updateGlobalPlacements(model,data)
- pin.computeAllTerms(model,data,q,np.zeros(model.nv))
+ pin.updateGlobalPlacements(model, data)
+ pin.computeAllTerms(model, data, q, np.zeros(model.nv))
time.sleep(3)
-
+
# vertex groups will have to be hardcoded per robot,
# there's no getting around it
vertices_grps = []
vertices_grps_indeces = [
- #[0, 1], [2], [3], [4,5,6,7,8,9,10]
- #[0, 1], [2], [3,4], [5,6,7,8,9,10]
- [0, 1], [2], [3,4], [5,6,7,8,9,10]
- ]
- vertices_grps_joint_parents = [collision_model.geometryObjects[0].parentJoint,
- collision_model.geometryObjects[2].parentJoint,
- collision_model.geometryObjects[3].parentJoint,
- #collision_model.geometryObjects[4].parentJoint]
- collision_model.geometryObjects[5].parentJoint]
+ # [0, 1], [2], [3], [4,5,6,7,8,9,10]
+ # [0, 1], [2], [3,4], [5,6,7,8,9,10]
+ [0, 1],
+ [2],
+ [3, 4],
+ [5, 6, 7, 8, 9, 10],
+ ]
+ vertices_grps_joint_parents = [
+ collision_model.geometryObjects[0].parentJoint,
+ collision_model.geometryObjects[2].parentJoint,
+ collision_model.geometryObjects[3].parentJoint,
+ # collision_model.geometryObjects[4].parentJoint]
+ collision_model.geometryObjects[5].parentJoint,
+ ]
# index of parent joint needs to be here
# we also maybe need parent frames
- #vertices_grps_joint_parents_indeces = [collision_model.geometryObjects[0].]
+ # vertices_grps_joint_parents_indeces = [collision_model.geometryObjects[0].]
# we'll np.vstack related vertices into vertices_grp
for v in vertices_grps_indeces:
# put them into their group
- vertices_grp = np.empty((0,3))
+ vertices_grp = np.empty((0, 3))
for j in v:
geom = collision_model.geometryObjects[j]
# plot em in meshcat
@@ -168,9 +192,11 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
vertix_pose = pin.SE3.Identity()
vertix_pose.translation = vertices[i]
- #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
- vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
- #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
+ # vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
+ vertix_pose = data.oMf[geom.parentFrame].act(
+ geom.placement.act(vertix_pose)
+ )
+ # viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
viz.applyConfiguration(f"world/point_{i}", vertix_pose)
vertices_in_joint_frame = []
@@ -181,20 +207,19 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
vertices_in_joint_frame.append(j_v)
vertices_in_joint_frame = np.array(vertices_in_joint_frame)
- #vertices_grp = np.vstack((vertices_grp, collision_model.geometryObjects[j]))
+ # vertices_grp = np.vstack((vertices_grp, collision_model.geometryObjects[j]))
vertices_grp = np.vstack((vertices_grp, vertices_in_joint_frame))
vertices_grps.append(vertices_grp)
-
ellipses = []
# go over grouped vertices and compute their ellipse fits
- #for i, geom in enumerate(collision_model.geometryObjects):
+ # for i, geom in enumerate(collision_model.geometryObjects):
# vertices = geom.geometry.vertices()
for i, vertices in enumerate(vertices_grps):
-
+
cw = casadi.SX.sym("w", 3)
exp = casadi.Function("exp3", [cw], [cpin.exp3(cw)])
-
+
"""
decide
- w in so3: ellipse orientation
@@ -210,63 +235,66 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
var_w = opti.variable(3)
var_r = opti.variable(3)
var_c = opti.variable(3)
-
+
# The ellipsoid matrix is represented by w=log3(R),diag(P) with R,P=eig(A)
R = exp(var_w)
A = R @ casadi.diag(1 / var_r**2) @ R.T
-
+
totalcost = var_r[0] * var_r[1] * var_r[2]
opti.subject_to(var_r >= 0)
-# for g_v in vertices:
+ # for g_v in vertices:
for j_v in vertices:
# g_v is the vertex v expressed in the geometry frame.
# Convert point from geometry frame to joint frame
-# print(j_v)
-# j_v = geom.placement.act(g_v)
+ # print(j_v)
+ # j_v = geom.placement.act(g_v)
# Constraint the ellipsoid to be including the point
opti.subject_to((j_v - var_c).T @ A @ (j_v - var_c) <= 1)
-
+
### SOLVE
opti.minimize(totalcost)
# remove prints i don't care about
- opts={}
+ opts = {}
opts["verbose_init"] = False
opts["verbose"] = False
opts["print_time"] = False
opts["ipopt.print_level"] = 0
opti.solver("ipopt", opts) # set numerical backend
opti.set_initial(var_r, 10)
-
+
sol = opti.solve_limited()
-
+
sol_r = opti.value(var_r)
sol_A = opti.value(A)
sol_c = opti.value(var_c)
sol_R = opti.value(exp(var_w))
-
+
# TODO: add placement=pin.SE3(P, ellipse.center) and id=robot.model.getJointId(e.name)
ellipse = SimpleNamespace(
- name=model.names[vertices_grps_joint_parents[i]],
- A=sol_A,
- center=sol_c)
+ name=model.names[vertices_grps_joint_parents[i]], A=sol_A, center=sol_c
+ )
ellipses.append(ellipse)
e, P = np.linalg.eig(sol_A)
ellipse_placement = pin.SE3(P, ellipse.center)
- #ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
- #ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
- ellipse_placement = data.oMi[vertices_grps_joint_parents[i]].act(ellipse_placement)
+ # ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
+ # ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ ellipse_placement = data.oMi[vertices_grps_joint_parents[i]].act(
+ ellipse_placement
+ )
viz.addEllipsoid(f"el_{ellipse.name}", sol_r, [0.3, 0.9, 0.3, 0.3])
viz.applyConfiguration(f"el_{ellipse.name}", ellipse_placement)
print(ellipse)
-
- ellipses_path = files('ur_simple_control.robot_descriptions.ellipses').joinpath("ur5e_robotiq_ellipses.pickle")
- file = open(ellipses_path, 'wb')
+
+ ellipses_path = files("smc.robot_descriptions.ellipses").joinpath(
+ "ur5e_robotiq_ellipses.pickle"
+ )
+ file = open(ellipses_path, "wb")
pickle.dump(ellipses, file)
file.close()
-
- # Recover r,R from A (for fun)
+
+ # Recover r,R from A (for fun)
# e, P = np.linalg.eig(sol_A)
# recons_r = 1 / e**0.5
# recons_R = P
@@ -292,25 +320,26 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
# + f" center=np.{repr(sol_c)})"
# )
# time.sleep(5)
- # Matplotlib (for fun)
- #import matplotlib.pyplot as plt
- #
- #plt.ion()
- #from utils.plot_ellipse import plotEllipse, plotVertices
-
- #fig, ax = plt.subplots(1, subplot_kw={"projection": "3d"})
- #plotEllipse(ax, sol_A, sol_c)
- #plotVertices(ax, np.vstack([geom.placement.act(p) for p in vertices]), 1)
+ # Matplotlib (for fun)
+ # import matplotlib.pyplot as plt
+ #
+ # plt.ion()
+ # from utils.plot_ellipse import plotEllipse, plotVertices
+
+ # fig, ax = plt.subplots(1, subplot_kw={"projection": "3d"})
+ # plotEllipse(ax, sol_A, sol_c)
+ # plotVertices(ax, np.vstack([geom.placement.act(p) for p in vertices]), 1)
def get_args():
parser = getMinimalArgParser()
- parser.description = 'describe this example'
+ parser.description = "describe this example"
# add more arguments here from different Simple Manipulator Control modules
args = parser.parse_args()
return args
-if __name__ == "__main__":
+
+if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
@@ -325,7 +354,7 @@ if __name__ == "__main__":
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
-
+
if args.save_log:
robot.log_manager.saveLog()
- #loop_manager.stopHandler(None, None)
+ # loop_manager.stopHandler(None, None)
diff --git a/python/smc/util/map2DPathTo3DPlane.py b/python/smc/util/map2DPathTo3DPlane.py
index afbad224c321bb7a9c7db105f7937e4d92c69d31..9d106acd22ff08ea1092f658205cf824a58de4f1 100644
--- a/python/smc/util/map2DPathTo3DPlane.py
+++ b/python/smc/util/map2DPathTo3DPlane.py
@@ -1,5 +1,6 @@
import numpy as np
+
#######################################################################
# map the pixels to a 3D plane #
#######################################################################
@@ -20,15 +21,13 @@ def map2DPathTo3DPlane(path_points_2D, width, height):
Returns a 3D path appropriately scaled, and placed into the first quadrant
of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
"""
- z = np.zeros((len(path_points_2D),1))
- path_points_3D = np.hstack((path_points_2D,z))
+ z = np.zeros((len(path_points_2D), 1))
+ path_points_3D = np.hstack((path_points_2D, z))
# scale the path to m
- path_points_3D[:,0] = path_points_3D[:,0] * width
- path_points_3D[:,1] = path_points_3D[:,1] * height
+ path_points_3D[:, 0] = path_points_3D[:, 0] * width
+ path_points_3D[:, 1] = path_points_3D[:, 1] * height
# in the new coordinate system we're going in the -y direction
- # TODO this is a demo specific hack,
+ # TODO this is a demo specific hack,
# make it general for a future release
- path_points_3D[:,1] = -1 * path_points_3D[:,1] + height
+ path_points_3D[:, 1] = -1 * path_points_3D[:, 1] + height
return path_points_3D
-
-
diff --git a/python/smc/visualize/meshcat_viewer_wrapper/__init__.py b/python/smc/visualization/__init__.py
similarity index 100%
rename from python/smc/visualize/meshcat_viewer_wrapper/__init__.py
rename to python/smc/visualization/__init__.py
diff --git a/python/smc/visualize/arms/another_debug_dh b/python/smc/visualization/arms/another_debug_dh
similarity index 100%
rename from python/smc/visualize/arms/another_debug_dh
rename to python/smc/visualization/arms/another_debug_dh
diff --git a/python/smc/visualize/arms/j2n6s300_dh_params b/python/smc/visualization/arms/j2n6s300_dh_params
similarity index 100%
rename from python/smc/visualize/arms/j2n6s300_dh_params
rename to python/smc/visualization/arms/j2n6s300_dh_params
diff --git a/python/smc/visualize/arms/j2s7s300_dh_params b/python/smc/visualization/arms/j2s7s300_dh_params
similarity index 100%
rename from python/smc/visualize/arms/j2s7s300_dh_params
rename to python/smc/visualization/arms/j2s7s300_dh_params
diff --git a/python/smc/visualize/arms/kinova_jaco_params b/python/smc/visualization/arms/kinova_jaco_params
similarity index 100%
rename from python/smc/visualize/arms/kinova_jaco_params
rename to python/smc/visualization/arms/kinova_jaco_params
diff --git a/python/smc/visualize/arms/kuka_lbw_iiwa_dh_params b/python/smc/visualization/arms/kuka_lbw_iiwa_dh_params
similarity index 100%
rename from python/smc/visualize/arms/kuka_lbw_iiwa_dh_params
rename to python/smc/visualization/arms/kuka_lbw_iiwa_dh_params
diff --git a/python/smc/visualize/arms/lbr_iiwa_jos_jedan_dh b/python/smc/visualization/arms/lbr_iiwa_jos_jedan_dh
similarity index 100%
rename from python/smc/visualize/arms/lbr_iiwa_jos_jedan_dh
rename to python/smc/visualization/arms/lbr_iiwa_jos_jedan_dh
diff --git a/python/smc/visualize/arms/robot_parameters2 b/python/smc/visualization/arms/robot_parameters2
similarity index 100%
rename from python/smc/visualize/arms/robot_parameters2
rename to python/smc/visualization/arms/robot_parameters2
diff --git a/python/smc/visualize/arms/testing_dh_parameters b/python/smc/visualization/arms/testing_dh_parameters
similarity index 100%
rename from python/smc/visualize/arms/testing_dh_parameters
rename to python/smc/visualization/arms/testing_dh_parameters
diff --git a/python/smc/visualize/arms/ur10e_dh_parameters_from_the_ur_site b/python/smc/visualization/arms/ur10e_dh_parameters_from_the_ur_site
similarity index 100%
rename from python/smc/visualize/arms/ur10e_dh_parameters_from_the_ur_site
rename to python/smc/visualization/arms/ur10e_dh_parameters_from_the_ur_site
diff --git a/python/smc/visualize/arms/ur5e_dh b/python/smc/visualization/arms/ur5e_dh
similarity index 100%
rename from python/smc/visualize/arms/ur5e_dh
rename to python/smc/visualization/arms/ur5e_dh
diff --git a/python/smc/visualize/manipulator_comparison_visualizer.py b/python/smc/visualization/manipulator_comparison_visualizer.py
similarity index 60%
rename from python/smc/visualize/manipulator_comparison_visualizer.py
rename to python/smc/visualization/manipulator_comparison_visualizer.py
index 11f018e2d1b079da3608245550520bd8ee8d03a3..9c1785ca85807110a8e48f41de63e33c349a93d8 100644
--- a/python/smc/visualize/manipulator_comparison_visualizer.py
+++ b/python/smc/visualization/manipulator_comparison_visualizer.py
@@ -1,23 +1,28 @@
+from smc.robots.implementations.simulated_robot import SimulatedRobotManager
+from smc.robots.utils import getMinimalArgParser
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.logging.logger import Logger
+from smc.visualization.visualizer import manipulatorComparisonVisualizer
+from smc.visualization.plotters import logPlotter
+
+from functools import partial
import numpy as np
-import argparse
import os
import time
from itertools import zip_longest
-from multiprocessing import Process, Queue
-from ur_simple_control.managers import getMinimalArgParser, RobotManager, ProcessManager
-from ur_simple_control.util.logging_utils import LogManager
-from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer, logPlotter
-from functools import partial
+
def getLogComparisonArgs():
parser = getMinimalArgParser()
-# parser = getClikArgs(parser)
-# parser = getDMPArgs(parser)
-# parser = getBoardCalibrationArgs(parser)
- parser.add_argument('--log-file1', type=str, \
- help="first log file to load for comparison")
- parser.add_argument('--log-file2', type=str, \
- help="first log file to load for comparison")
+ # parser = getClikArgs(parser)
+ # parser = getDMPArgs(parser)
+ # parser = getBoardCalibrationArgs(parser)
+ parser.add_argument(
+ "--log-file1", type=str, help="first log file to load for comparison"
+ )
+ parser.add_argument(
+ "--log-file2", type=str, help="first log file to load for comparison"
+ )
args = parser.parse_args()
# these are obligatory
args.visualize_manipulator = False
@@ -25,6 +30,7 @@ def getLogComparisonArgs():
args.simulation = False
return args
+
# TODO: use ProcessManager for this
class ManipulatorComparisonManager:
def __init__(self, args):
@@ -34,8 +40,8 @@ class ManipulatorComparisonManager:
args.pinocchio_only = True
args.simulation = False
- self.robot1 = RobotManager(args)
- self.robot2 = RobotManager(args)
+ self.robot1 = SimulatedRobotManager(args)
+ self.robot2 = SimulatedRobotManager(args)
# no two loops will have the same amount of timesteps.
# we need to store the last available step for both robots.
@@ -55,11 +61,14 @@ class ManipulatorComparisonManager:
print("you did not give me a valid path for log2, exiting")
exit()
- side_function = partial(manipulatorComparisonVisualizer, self.robot1.model,
- self.robot1.visual_model, self.robot1.collision_model)
+ side_function = partial(
+ manipulatorComparisonVisualizer,
+ self.robot1.model,
+ self.robot1.visual_model,
+ self.robot1.collision_model,
+ )
cmd = (np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq))
- self.visualizer_manager = ProcessManager(args, side_function,
- cmd, 2)
+ self.visualizer_manager = ProcessManager(args, side_function, cmd, 2)
# wait until it's ready (otherwise you miss half the sim potentially)
# 5 seconds should be more than enough,
# and i don't want to complicate this code by complicating IPC
@@ -71,7 +80,6 @@ class ManipulatorComparisonManager:
###########################################
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.
@@ -85,41 +93,53 @@ class ManipulatorComparisonManager:
self.plotter_manager.getData()
def createRunningPlot(self, log, log_plotter_time_start, log_plotter_time_stop):
- self.log_plotters.append(self.RunningPlotter(self.args, log, log_plotter_time_start, log_plotter_time_stop))
+ 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.visualizer_manager.sendCommand((q1, q2))
+ self.visualizer_manager.sendCommand({"q1": q1, "q2": q2})
for log_plotter in self.log_plotters:
- if (time_index >= log_plotter.time_start) and (time_index < log_plotter.time_stop):
- log_plotter.plotter_manager.sendCommand(time_index - log_plotter.time_start)
+ if (time_index >= log_plotter.time_start) and (
+ time_index < log_plotter.time_stop
+ ):
+ log_plotter.plotter_manager.sendCommand(
+ time_index - log_plotter.time_start
+ )
log_plotter.plotter_manager.getData()
self.visualizer_manager.getData()
# NOTE: this uses slightly fancy python to make it bareable to code
# NOTE: dict keys are GUARANTEED to be in insert order from python 3.7 onward
def visualizeWholeRuns(self):
- time_index = 0
- for control_loop1, control_loop2 in zip_longest(self.logm1.loop_logs, self.logm2.loop_logs):
- print(f'run {self.logm1.args.run_name}, controller: {control_loop1}')
- print(f'run {self.logm2.args.run_name}, controller: {control_loop2}')
+ time_index = 0
+ for control_loop1, control_loop2 in zip_longest(
+ self.logm1.loop_logs, self.logm2.loop_logs
+ ):
+ print(f"run {self.logm1.args.run_name}, controller: {control_loop1}")
+ print(f"run {self.logm2.args.run_name}, controller: {control_loop2}")
# relying on python's default thing.toBool()
if not control_loop1:
print(f"run {self.logm1.args.run_name} is finished")
q1 = self.lastq1
- for q2 in self.logm2.loop_logs[control_loop2]['qs']:
+ for q2 in self.logm2.loop_logs[control_loop2]["qs"]:
self.updateViz(q1, q2, time_index)
time_index += 1
print(f"run {self.logm2.args.run_name} is finished")
if not control_loop2:
print(f"run {self.logm2.args.run_name} is finished")
q2 = self.lastq2
- for q1 in self.logm1.loop_logs[control_loop1]['qs']:
+ for q1 in self.logm1.loop_logs[control_loop1]["qs"]:
self.updateViz(q1, q2, time_index)
time_index += 1
print(f"run {self.logm1.args.run_name} is finished")
if control_loop1 and control_loop2:
- for q1, q2 in zip_longest(self.logm1.loop_logs[control_loop1]['qs'], \
- self.logm2.loop_logs[control_loop2]['qs']):
+ for q1, q2 in zip_longest(
+ self.logm1.loop_logs[control_loop1]["qs"],
+ self.logm2.loop_logs[control_loop2]["qs"],
+ ):
if not (q1 is None):
self.lastq1 = q1
if not (q2 is None):
@@ -128,18 +148,17 @@ class ManipulatorComparisonManager:
time_index += 1
-
# TODO: update
if __name__ == "__main__":
args = getLogComparisonArgs()
- cmp_manager = ManipulatorComparisonManager(args)
- log_plot = {'random_noise' : np.random.normal(size=(1000, 2))}
- cmp_manager.createRunningPlot(log_plot, 200, 1200)
- cmp_manager.visualizeWholeRuns()
- time.sleep(100)
- 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")
+# cmp_manager = ManipulatorComparisonManager(args)
+# log_plot = {"random_noise": np.random.normal(size=(1000, 2))}
+# cmp_manager.createRunningPlot(log_plot, 200, 1200)
+# cmp_manager.visualizeWholeRuns()
+# time.sleep(100)
+# 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/smc/visualize/manipulator_visual_motion_analyzer.py b/python/smc/visualization/manipulator_visual_motion_analyzer.py
similarity index 99%
rename from python/smc/visualize/manipulator_visual_motion_analyzer.py
rename to python/smc/visualization/manipulator_visual_motion_analyzer.py
index 61182930c12379c14d7521597232f02cf33ca755..3b9a4254c4fbc6a2514384abd03fd665749c9496 100644
--- a/python/smc/visualize/manipulator_visual_motion_analyzer.py
+++ b/python/smc/visualization/manipulator_visual_motion_analyzer.py
@@ -17,9 +17,9 @@ from matplotlib.gridspec import GridSpec
from robot_stuff.InverseKinematics import InverseKinematicsEnv
from robot_stuff.drawing import *
from robot_stuff.inv_kinm import *
-from ur_simple_control.visualize.make_run import makeRun, loadRun
-from ur_simple_control.managers import RobotManager
-from ur_simple_control.util.logging_utils import loadRunForAnalysis, cleanUpRun
+from smc.visualize.make_run import makeRun, loadRun
+from smc.managers import RobotManager
+from smc.logging.logger import loadRunForAnalysis, cleanUpRun
import numpy as np
# it is the best solution for a particular problem
@@ -876,8 +876,8 @@ if __name__ == "__main__":
root = Tk()
# TODO: change to something different obviously
# or add a button to load or something, idc
- log_data_file_name = "/home/gospodar/lund/praxis/projects/ur_simple_control/python/examples/data/clik_run_001.pickle"
- args_file_name = "/home/gospodar/lund/praxis/projects/ur_simple_control/python/examples/data/clik_run_001_args.pickle"
+ log_data_file_name = "/home/gospodar/lund/praxis/projects/smc/python/examples/data/clik_run_001.pickle"
+ args_file_name = "/home/gospodar/lund/praxis/projects/smc/python/examples/data/clik_run_001_args.pickle"
log_data, args = loadRunForAnalysis(log_data_file_name, args_file_name)
args.visualize_manipulator = False
log_data = cleanUpRun(log_data, log_data['qs'].shape[0], 200)
diff --git a/python/smc/visualization/meshcat_viewer_wrapper/__init__.py b/python/smc/visualization/meshcat_viewer_wrapper/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/smc/visualize/meshcat_viewer_wrapper/colors.py b/python/smc/visualization/meshcat_viewer_wrapper/colors.py
similarity index 100%
rename from python/smc/visualize/meshcat_viewer_wrapper/colors.py
rename to python/smc/visualization/meshcat_viewer_wrapper/colors.py
diff --git a/python/smc/visualize/meshcat_viewer_wrapper/visualizer.py b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
similarity index 100%
rename from python/smc/visualize/meshcat_viewer_wrapper/visualizer.py
rename to python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
diff --git a/python/smc/visualize/obsolete_to_be_removed/main.py b/python/smc/visualization/obsolete_to_be_removed/main.py
similarity index 100%
rename from python/smc/visualize/obsolete_to_be_removed/main.py
rename to python/smc/visualization/obsolete_to_be_removed/main.py
diff --git a/python/smc/visualize/obsolete_to_be_removed/make_run.py b/python/smc/visualization/obsolete_to_be_removed/make_run.py
similarity index 100%
rename from python/smc/visualize/obsolete_to_be_removed/make_run.py
rename to python/smc/visualization/obsolete_to_be_removed/make_run.py
diff --git a/python/smc/visualize/visualize.py b/python/smc/visualization/plotters.py
similarity index 51%
rename from python/smc/visualize/visualize.py
rename to python/smc/visualization/plotters.py
index 862d3bd585a676ee6597129e874c3f8f6b744260..ffe8253d1d09335070fcda90db775a6e468279d3 100644
--- a/python/smc/visualize/visualize.py
+++ b/python/smc/visualization/plotters.py
@@ -1,28 +1,16 @@
import numpy as np
import matplotlib.pyplot as plt
from collections import deque, namedtuple
-import time
-import copy
-#from pinocchio.visualize import MeshcatVisualizer
-# TODO: use wrapped meshcat visualizer to have access to more nifty plotting
-from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
-from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat
-import meshcat_shapes
-# 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):
- if n_plots == 1:
- n_cols = 1
- n_rows = 1
+ n_cols = 1
+ n_rows = 1
if n_plots == 2:
n_rows = 2
n_cols = 1
- # i'm not going to bother with differently sized plots
+ # i'm not going to bother with differently sized plots
if (n_plots == 3) or (n_plots == 4):
n_cols = 2
n_rows = 2
@@ -33,24 +21,26 @@ def getNRowsMColumnsFromTotalNumber(n_plots):
n_cols = 3
n_rows = 3
if n_plots >= 10:
- raise NotImplementedError("sorry, you can only do up to 9 plots. more require tabs, and that's future work")
+ raise NotImplementedError(
+ "sorry, you can only do up to 9 plots. more require tabs, and that's future work"
+ )
return n_rows, n_cols
def plotFromDict(plot_data, final_iteration, args, title="title"):
- """
+ """
plotFromDict
------------
plots logs stored in a dictionary
- every key is one subplot, and it's value
is what you plot
- """
+ """
# TODO: replace with actual time ( you know what dt is from args)
t = np.arange(final_iteration)
n_cols, n_rows = getNRowsMColumnsFromTotalNumber(len(plot_data))
# this is what subplot wants
subplot_col_row = str(n_cols) + str(n_rows)
- ax_dict ={}
+ ax_dict = {}
# NOTE: cutting off after final iterations is a vestige from a time
# when logs were prealocated arrays, but it ain't hurtin' nobody as it is
plt.title(title)
@@ -58,7 +48,12 @@ def plotFromDict(plot_data, final_iteration, args, title="title"):
colors = plt.cm.jet(np.linspace(0, 1, plot_data[data_key].shape[1]))
ax_dict[data_key] = plt.subplot(int(subplot_col_row + str(i + 1)))
for j in range(plot_data[data_key].shape[1]):
- 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].plot(
+ t,
+ plot_data[data_key][:final_iteration, j],
+ color=colors[j],
+ label=data_key + "_" + str(j),
+ )
ax_dict[data_key].legend()
plt.show()
@@ -78,20 +73,20 @@ def realTimePlotter(args, log_item, queue):
plt.ion()
fig = plt.figure()
canvas = fig.canvas
-# if args.debug_prints:
-# print("REAL_TIME_PLOTTER: putting success into queue")
- #queue.put("success")
+ # if args.debug_prints:
+ # print("REAL_TIME_PLOTTER: putting success into queue")
+ # queue.put("success")
logs_deque = {}
logs_ndarrays = {}
AxisAndArtists = namedtuple("AxAndArtists", "ax artists")
axes_and_updating_artists = {}
- #if args.debug_prints:
+ # if args.debug_prints:
# print("REAL_TIME_PLOTTER: i am waiting for the first log_item to initialize myself")
- #log_item = queue.get()
+ # log_item = queue.get()
if len(log_item) == 0:
print("you've send me nothing, so no real-time plotting for you")
return
- #if args.debug_prints:
+ # if args.debug_prints:
# print("REAL_TIME_PLOTTER: got log_item, i am initializing the desired plots")
ROLLING_BUFFER_SIZE = 100
t = np.arange(ROLLING_BUFFER_SIZE)
@@ -101,38 +96,45 @@ def realTimePlotter(args, log_item, queue):
subplot_col_row = str(n_cols) + str(n_rows)
# preload some zeros and initialize plots
for i, data_key in enumerate(log_item):
- # you give single-vector numpy arrays, i instantiate and plot lists of these
+ # you give single-vector numpy arrays, i instantiate and plot lists of these
# so for your (6,) vector, i plot (N, 6) ndarrays resulting in 6 lines of length N.
# i manage N because plot data =/= all data for efficiency reasons.
assert type(log_item[data_key]) == np.ndarray
assert len(log_item[data_key].shape) == 1
- # prepopulate with zeros via list comperhension (1 item is the array, the queue is
+ # prepopulate with zeros via list comperhension (1 item is the array, the queue is
# ROLLING_BUFFER_SIZE of such arrays, and these go in and out at every time step)
- logs_deque[data_key] = deque([log_item[data_key] for index in range(ROLLING_BUFFER_SIZE)])
+ logs_deque[data_key] = deque(
+ [log_item[data_key] for _ in range(ROLLING_BUFFER_SIZE)]
+ )
# i can only plot np_arrays, so these queues will have to be turned to nparray at every timestep
# thankfull, deque is an iterable
logs_ndarrays[data_key] = np.array(logs_deque[data_key])
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 'qs' in data_key:
+ if "qs" in data_key:
ax.set_ylim(bottom=-6.14, top=6.14)
- if 'vs' in data_key:
+ if "vs" in data_key:
ax.set_ylim(bottom=-3.14, top=3.14)
- if 'wrench' in data_key:
+ if "wrench" in data_key:
ax.set_ylim(bottom=-20.0, top=20.0)
- if 'tau' in data_key:
+ if "tau" in data_key:
ax.set_ylim(bottom=-2.0, top=2.0)
- if 'err' in data_key:
+ if "err" 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]):
# the comma is because plot retuns ax, sth_unimportant.
# and python let's us assign iterable return values like this
- axes_and_updating_artists[data_key].artists[str(data_key) + str(j)], = \
- axes_and_updating_artists[data_key].ax.plot(t, logs_ndarrays[data_key][:,j],
- color=colors[j], label=data_key + "_" + str(j))
- axes_and_updating_artists[data_key].ax.legend(loc='upper left')
+ (
+ axes_and_updating_artists[data_key].artists[str(data_key) + str(j)],
+ ) = axes_and_updating_artists[data_key].ax.plot(
+ t,
+ logs_ndarrays[data_key][:, j],
+ color=colors[j],
+ label=data_key + "_" + str(j),
+ )
+ axes_and_updating_artists[data_key].ax.legend(loc="upper left")
# need to call it once
canvas.draw()
@@ -157,9 +159,14 @@ def realTimePlotter(args, log_item, queue):
logs_ndarrays[data_key] = np.array(logs_deque[data_key])
# now shape == (ROLLING_BUFFER_SIZE, vector_dimension)
for j in range(logs_ndarrays[data_key].shape[1]):
- axes_and_updating_artists[data_key].artists[str(data_key) + str(j)].set_data(t, logs_ndarrays[data_key][:,j])
- axes_and_updating_artists[data_key].ax.draw_artist(\
- axes_and_updating_artists[data_key].artists[str(data_key) + str(j)])
+ axes_and_updating_artists[data_key].artists[
+ str(data_key) + str(j)
+ ].set_data(t, logs_ndarrays[data_key][:, j])
+ axes_and_updating_artists[data_key].ax.draw_artist(
+ axes_and_updating_artists[data_key].artists[
+ str(data_key) + str(j)
+ ]
+ )
canvas.blit(fig.bbox)
canvas.flush_events()
except KeyboardInterrupt:
@@ -168,149 +175,9 @@ def realTimePlotter(args, log_item, queue):
plt.close(fig)
-def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue):
- viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
- viz.loadViewerModel()
- # display the initial pose
- viz.display(cmd["q"])
- # set shapes we know we'll use
- meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
- meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
- meshcat_shapes.frame(viz.viewer["T_base"], opacity=0.5)
- print("MANIPULATORVISUALIZER: FULLY ONLINE")
- try:
- while True:
- 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
- if key == "Mgoal":
- viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
- if key == "T_w_e":
- viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
- if key == "T_base":
- viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous)
- if key == "q":
- viz.display(cmd["q"])
- if key == "point":
- viz.addPoint(cmd["point"])
- if key == "obstacle_sphere":
- # stupid and evil but there is no time
- viz.addSphereObstacle(cmd["obstacle_sphere"][0], cmd["obstacle_sphere"][1])
- if key == "obstacle_box":
- # stupid and evil but there is no time
- viz.addBoxObstacle(cmd["obstacle_box"][0], cmd["obstacle_box"][1])
- if key == "path":
- # stupid and evil but there is no time
- viz.addPath("", cmd["path"])
- if key == "frame_path":
- # stupid and evil but there is no time
- viz.addFramePath("", cmd["frame_path"])
-
- except KeyboardInterrupt:
- if args.debug_prints:
- print("MANIPULATORVISUALIZER: caught KeyboardInterrupt, i'm out")
- viz.viewer.window.server_proc.kill()
- viz.viewer.window.server_proc.wait()
-
-# could be merged with the above function.
-# but they're different enough in usage to warrent a different function,
-# instead of polluting the above one with ifs
-def manipulatorComparisonVisualizer(model, collision_model, visual_model, args, cmd, cmd_queue, ack_queue):
- for geom in visual_model.geometryObjects:
- if "hand" in geom.name:
- geom.meshColor = np.array([0.2,0.2,0.2,0.2])
- viz = MeshcatVisualizer(model=model,
- collision_model=collision_model,
- visual_model=visual_model)
- viz.initViewer(open=True)
- # load the first one
- viz.loadViewerModel(collision_color=[0.2,0.2,0.2,0.6])
- #viz.viewer["pinocchio/visuals"].set_property("visible", False)
- #viz.viewer["pinocchio/collisions"].set_property("visible", True)
- viz.displayVisuals(False)
- viz.displayCollisions(True)
- # maybe needed
- #viz.displayVisuals(True)
-# meshpath = viz.viewerVisualGroupName
-# for geom in visual_model(6):
-# meshpath_i = meshpath + "/" + geometry_object.name
- #viz.viewer["pinocchio/visuals"].set_property("opacity", 0.2)
- #viz.viewer["pinocchio"].set_property("opacity", 0.2)
- #viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("opacity", 0.2)
-
- #viz.viewer["pinocchio"].set_property("color", (0.2,0.2,0.2,0.2))
- #viz.viewer["pinocchio/visuals"].set_property("color",(0.2,0.2,0.2,0.2))
- #viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("color", (0.2,0.2,0.2,0.2))
-
- ## this is the path we want, with the /<object> at the end
- #node = viz.viewer["pinocchio/visuals/forearm_link_0/<object>"]
- #print(node)
- #node.set_property("opacity", 0.2)
- #node.set_property("modulated_opacity", 0.2)
- #node.set_property("color", [0.2] * 4)
- #node.set_property("scale", 100.2)
- # this one actually works
- #node.set_property("visible", False)
-
- node = viz.viewer["pinocchio/visuals"]
- #node.set_property("visible", False)
- node.set_property("modulated_opacity", 0.4)
- node.set_property("opacity", 0.2)
- node.set_property("color", [0.2] * 4)
-
- #meshcat->SetProperty("path/to/my/thing/<object>", "opacity", alpha);
-
-
- # other robot display
- viz2 = UnWrappedMeshcat(model=model,
- collision_model=collision_model,
- visual_model=visual_model)
- viz2.initViewer(viz.viewer)
- # i don't know if rootNodeName does anything apart from being different
- #viz2.loadViewerModel(rootNodeName="pinocchio2", visual_color=(1.0,1.0,1.0,0.1))
- viz2.loadViewerModel(rootNodeName="pinocchio2")
- # initialize
- q1, q2 = cmd_queue.get()
- viz.display(q1)
- viz2.display(q2)
-
- ack_queue.put("ready")
- print("MANIPULATOR_COMPARISON_VISUALIZER: FULLY ONLINE")
- try:
- while True:
- q = cmd_queue.get()
- if type(q) == str:
- print("got str q")
- if q == "befree":
- if args.debug_prints:
- print("MANIPULATOR_COMPARISON_VISUALIZER: got befree, manipulatorComparisonVisualizer out")
- viz.viewer.window.server_proc.kill()
- viz.viewer.window.server_proc.wait()
- break
- q1, q2 = q
- viz.display(q1)
- viz2.display(q2)
- # this doesn't really work because meshcat is it's own server
- # and display commands just relay their command and return immediatelly.
- # but it's better than nothing.
- # NOTE: if there's lag in meshcat, just add a small sleep here before the
- # ack signal - that will ensure synchronization because meshat will actually be ready
- ack_queue.put("ready")
- except KeyboardInterrupt:
- if args.debug_prints:
- print("MANIPULATOR_COMPARISON_VISUALIZER: caught KeyboardInterrupt, i'm out")
- viz.viewer.window.server_proc.kill()
- 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
+# 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,
@@ -322,8 +189,8 @@ def logPlotter(log, args, cmd, cmd_queue, ack_queue):
logPlotter
---------------
- plots whatever you want as long as you pass the data
- as a dictionary where the key will be the name on the plot,
- and the value have to be the dependent variables -
+ as a dictionary where the key will be the name on the plot,
+ and the value have to be the dependent variables -
the independent variable is time and has to be the same for all items.
if you want to plot something else, you need to write your own function.
use this as a skeleton if you want that new plot to be updating too
@@ -345,7 +212,7 @@ def logPlotter(log, args, cmd, cmd_queue, ack_queue):
subplot_col_row = str(n_cols) + str(n_rows)
# preload some zeros and initialize plots
for i, data_key in enumerate(log):
- # you give single-vector numpy arrays, i instantiate and plot lists of these
+ # you give single-vector numpy arrays, i instantiate and plot lists of these
# so for your (6,) vector, i plot (N, 6) ndarrays resulting in 6 lines of length N.
# i manage N because plot data =/= all data for efficiency reasons.
assert type(log[data_key]) == np.ndarray
@@ -359,13 +226,17 @@ def logPlotter(log, args, cmd, cmd_queue, ack_queue):
for j in range(log[data_key].shape[1]):
# NOTE the same length assumption plays a part for correctness,
# but i don't want that to be an error in case you know what you're doing
- ax.plot(np.arange(len(log[data_key])), log[data_key][:,j],
- color=colors[j], label=data_key + "_" + str(j))
+ ax.plot(
+ np.arange(len(log[data_key])),
+ log[data_key][:, j],
+ color=colors[j],
+ label=data_key + "_" + str(j),
+ )
# vertical bar does update
- point_in_time_line = ax.axvline(x=0, color='red', animated=True)
+ point_in_time_line = ax.axvline(x=0, color="red", animated=True)
axes_and_updating_artists[data_key] = AxisAndArtists(ax, point_in_time_line)
- axes_and_updating_artists[data_key].ax.legend(loc='upper left')
+ axes_and_updating_artists[data_key].ax.legend(loc="upper left")
# need to call it once to start, more if something breaks
canvas.draw()
@@ -373,7 +244,7 @@ def logPlotter(log, args, cmd, cmd_queue, ack_queue):
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)
+ # cid = fig.canvas.mpl_connect('button_press_event', onclick)
def onEvent(event):
print("drawing")
canvas.draw()
@@ -381,8 +252,7 @@ def logPlotter(log, args, cmd, cmd_queue, ack_queue):
background = canvas.copy_from_bbox(fig.bbox)
print("copied canvas")
- cid = fig.canvas.mpl_connect('button_press_event', onEvent)
-
+ cid = fig.canvas.mpl_connect("button_press_event", onEvent)
ack_queue.put("ready")
if args.debug_prints:
@@ -399,9 +269,11 @@ def logPlotter(log, args, cmd, cmd_queue, ack_queue):
canvas.restore_region(background)
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)
+ axes_and_updating_artists[data_key].ax.draw_artist(
+ axes_and_updating_artists[data_key].artists
+ )
# NOTE: this is stupid, i just want to catch the resize event
- #if not (counter % 50 == 0):
+ # if not (counter % 50 == 0):
if True:
canvas.blit(fig.bbox)
canvas.flush_events()
diff --git a/python/smc/visualize/robot_stuff/InverseKinematics.py b/python/smc/visualization/robot_stuff/InverseKinematics.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/InverseKinematics.py
rename to python/smc/visualization/robot_stuff/InverseKinematics.py
diff --git a/python/smc/visualize/robot_stuff/drawing.py b/python/smc/visualization/robot_stuff/drawing.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/drawing.py
rename to python/smc/visualization/robot_stuff/drawing.py
diff --git a/python/smc/visualize/robot_stuff/drawing_for_anim.py b/python/smc/visualization/robot_stuff/drawing_for_anim.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/drawing_for_anim.py
rename to python/smc/visualization/robot_stuff/drawing_for_anim.py
diff --git a/python/smc/visualize/robot_stuff/follow_curve.py b/python/smc/visualization/robot_stuff/follow_curve.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/follow_curve.py
rename to python/smc/visualization/robot_stuff/follow_curve.py
diff --git a/python/smc/visualize/robot_stuff/forw_kinm.py b/python/smc/visualization/robot_stuff/forw_kinm.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/forw_kinm.py
rename to python/smc/visualization/robot_stuff/forw_kinm.py
diff --git a/python/smc/visualize/robot_stuff/inv_kinm.py b/python/smc/visualization/robot_stuff/inv_kinm.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/inv_kinm.py
rename to python/smc/visualization/robot_stuff/inv_kinm.py
diff --git a/python/smc/visualize/robot_stuff/joint_as_hom_mat.py b/python/smc/visualization/robot_stuff/joint_as_hom_mat.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/joint_as_hom_mat.py
rename to python/smc/visualization/robot_stuff/joint_as_hom_mat.py
diff --git a/python/smc/visualize/robot_stuff/utils.py b/python/smc/visualization/robot_stuff/utils.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/utils.py
rename to python/smc/visualization/robot_stuff/utils.py
diff --git a/python/smc/visualize/robot_stuff/webots_api_helper_funs.py b/python/smc/visualization/robot_stuff/webots_api_helper_funs.py
similarity index 100%
rename from python/smc/visualize/robot_stuff/webots_api_helper_funs.py
rename to python/smc/visualization/robot_stuff/webots_api_helper_funs.py
diff --git a/python/smc/visualization/visualizer.py b/python/smc/visualization/visualizer.py
new file mode 100644
index 0000000000000000000000000000000000000000..98c07a28d1aa31e26ba92af222c7fae7073ce972
--- /dev/null
+++ b/python/smc/visualization/visualizer.py
@@ -0,0 +1,161 @@
+import numpy as np
+from smc.visualization.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
+from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat
+import meshcat_shapes
+
+# tkinter stuff for later reference
+# from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk)
+# from tkinter import *
+# from tkinter.ttk import *
+
+
+def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue):
+ viz = MeshcatVisualizer(
+ model=model, collision_model=collision_model, visual_model=visual_model
+ )
+ viz.loadViewerModel()
+ # display the initial pose
+ viz.display(cmd["q"])
+ # set shapes we know we'll use
+ meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
+ meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
+ meshcat_shapes.frame(viz.viewer["T_base"], opacity=0.5)
+ print("MANIPULATORVISUALIZER: FULLY ONLINE")
+ try:
+ while True:
+ 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
+ if key == "Mgoal":
+ viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
+ if key == "T_w_e":
+ viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
+ if key == "T_base":
+ viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous)
+ if key == "q":
+ viz.display(cmd["q"])
+ if key == "point":
+ viz.addPoint(cmd["point"])
+ if key == "obstacle_sphere":
+ # stupid and evil but there is no time
+ viz.addSphereObstacle(
+ cmd["obstacle_sphere"][0], cmd["obstacle_sphere"][1]
+ )
+ if key == "obstacle_box":
+ # stupid and evil but there is no time
+ viz.addBoxObstacle(cmd["obstacle_box"][0], cmd["obstacle_box"][1])
+ if key == "path":
+ # stupid and evil but there is no time
+ viz.addPath("", cmd["path"])
+ if key == "frame_path":
+ # stupid and evil but there is no time
+ viz.addFramePath("", cmd["frame_path"])
+
+ except KeyboardInterrupt:
+ if args.debug_prints:
+ print("MANIPULATORVISUALIZER: caught KeyboardInterrupt, i'm out")
+ viz.viewer.window.server_proc.kill()
+ viz.viewer.window.server_proc.wait()
+
+
+# could be merged with the above function.
+# but they're different enough in usage to warrent a different function,
+# instead of polluting the above one with ifs
+def manipulatorComparisonVisualizer(
+ model, collision_model, visual_model, args, cmd, cmd_queue, ack_queue
+):
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ geom.meshColor = np.array([0.2, 0.2, 0.2, 0.2])
+ viz = MeshcatVisualizer(
+ model=model, collision_model=collision_model, visual_model=visual_model
+ )
+ viz.initViewer(open=True)
+ # load the first one
+ viz.loadViewerModel(collision_color=[0.2, 0.2, 0.2, 0.6])
+ # viz.viewer["pinocchio/visuals"].set_property("visible", False)
+ # viz.viewer["pinocchio/collisions"].set_property("visible", True)
+ viz.displayVisuals(False)
+ viz.displayCollisions(True)
+ # maybe needed
+ # viz.displayVisuals(True)
+ # meshpath = viz.viewerVisualGroupName
+ # for geom in visual_model(6):
+ # meshpath_i = meshpath + "/" + geometry_object.name
+ # viz.viewer["pinocchio/visuals"].set_property("opacity", 0.2)
+ # viz.viewer["pinocchio"].set_property("opacity", 0.2)
+ # viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("opacity", 0.2)
+
+ # viz.viewer["pinocchio"].set_property("color", (0.2,0.2,0.2,0.2))
+ # viz.viewer["pinocchio/visuals"].set_property("color",(0.2,0.2,0.2,0.2))
+ # viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("color", (0.2,0.2,0.2,0.2))
+
+ ## this is the path we want, with the /<object> at the end
+ # node = viz.viewer["pinocchio/visuals/forearm_link_0/<object>"]
+ # print(node)
+ # node.set_property("opacity", 0.2)
+ # node.set_property("modulated_opacity", 0.2)
+ # node.set_property("color", [0.2] * 4)
+ # node.set_property("scale", 100.2)
+ # this one actually works
+ # node.set_property("visible", False)
+
+ node = viz.viewer["pinocchio/visuals"]
+ # node.set_property("visible", False)
+ node.set_property("modulated_opacity", 0.4)
+ node.set_property("opacity", 0.2)
+ node.set_property("color", [0.2] * 4)
+
+ # meshcat->SetProperty("path/to/my/thing/<object>", "opacity", alpha);
+
+ # other robot display
+ viz2 = UnWrappedMeshcat(
+ model=model, collision_model=collision_model, visual_model=visual_model
+ )
+ viz2.initViewer(viz.viewer)
+ # i don't know if rootNodeName does anything apart from being different
+ # viz2.loadViewerModel(rootNodeName="pinocchio2", visual_color=(1.0,1.0,1.0,0.1))
+ viz2.loadViewerModel(rootNodeName="pinocchio2")
+ # initialize
+ q1, q2 = cmd_queue.get()
+ viz.display(q1)
+ viz2.display(q2)
+
+ ack_queue.put("ready")
+ print("MANIPULATOR_COMPARISON_VISUALIZER: FULLY ONLINE")
+ try:
+ while True:
+ q = cmd_queue.get()
+ if type(q) == str:
+ print("got str q")
+ if q == "befree":
+ if args.debug_prints:
+ print(
+ "MANIPULATOR_COMPARISON_VISUALIZER: got befree, manipulatorComparisonVisualizer out"
+ )
+ viz.viewer.window.server_proc.kill()
+ viz.viewer.window.server_proc.wait()
+ break
+ q1, q2 = q
+ viz.display(q1)
+ viz2.display(q2)
+ # this doesn't really work because meshcat is it's own server
+ # and display commands just relay their command and return immediatelly.
+ # but it's better than nothing.
+ # NOTE: if there's lag in meshcat, just add a small sleep here before the
+ # ack signal - that will ensure synchronization because meshat will actually be ready
+ ack_queue.put("ready")
+ except KeyboardInterrupt:
+ if args.debug_prints:
+ print(
+ "MANIPULATOR_COMPARISON_VISUALIZER: caught KeyboardInterrupt, i'm out"
+ )
+ viz.viewer.window.server_proc.kill()
+ viz.viewer.window.server_proc.wait()