diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 35e0e69578300d20e21368dae3ffce306c5acb26..a0b8d642c6528d79754ac34332f0e72a11473710 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -15,7 +15,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
@@ -295,7 +294,7 @@ def controller():
# to find option 3)
# control loop to be passed to ControlLoopManager
-def controlLoopWriting(wrench_offset, dmp, tc, controller, robot, i, past_data):
+def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -325,7 +324,6 @@ def controlLoopWriting(wrench_offset, dmp, tc, controller, robot, i, past_data):
#Z = np.diag(np.array([1.0, 0.6, 1.0, 0.5, 0.5, 0.5]))
wrench = robot.getWrench()
- wrench = wrench - wrench_offset
# deepcopy for good coding practise (and correctness here)
save_past_dict['wrench'] = copy.deepcopy(wrench)
# rolling average
@@ -387,8 +385,8 @@ if __name__ == "__main__":
robot = RobotManager(args)
# calibrate FT first
- wrench_offset = robot.calibrateFT()
- robot.wrench_offset = wrench_offset
+ # it's done by default now because it's basically always necessary
+
#######################################################################
# drawing a path, making a joint trajectory for it #
#######################################################################
@@ -396,7 +394,7 @@ if __name__ == "__main__":
# draw the path on the screen
if args.draw_new:
- pixel_path = drawPath()
+ pixel_path = drawPath(args)
# make it 3D
else:
pixel_path_file_path = './path_in_pixels.csv'
@@ -523,7 +521,7 @@ if __name__ == "__main__":
'dqs' : np.zeros((args.max_iterations, 6)),
'dmp_vels' : np.zeros((args.max_iterations, 6)),
}
- controlLoop = partial(controlLoopWriting, wrench_offset, dmp, tc, controller, robot)
+ controlLoop = partial(controlLoopWriting, dmp, tc, controller, robot)
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
#######################################################################
# physical setup #
diff --git a/python/examples/planar_dragging_via_top_contact_force.py b/python/examples/planar_dragging_via_top_contact_force.py
index 653485e12bab015a78ed1aa23026942f106f4259..171b4442cbc4c188dda526cbb45aa8e33105325a 100644
--- a/python/examples/planar_dragging_via_top_contact_force.py
+++ b/python/examples/planar_dragging_via_top_contact_force.py
@@ -5,7 +5,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
@@ -153,7 +152,7 @@ def controller():
# control loop to be passed to ControlLoopManager
-def controlLoopPlanarDragging(wrench_offset, dmp, tc, controller, robot, i, past_data):
+def controlLoopPlanarDragging(dmp, tc, controller, robot, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -166,7 +165,6 @@ def controlLoopPlanarDragging(wrench_offset, dmp, tc, controller, robot, i, past
Z = np.diag(np.ones(6))
wrench = robot.getWrench()
- wrench = wrench - wrench_offset
wrench = np.average(np.array(past_data['wrench']), axis=0)
# first-order low pass filtering
@@ -175,7 +173,7 @@ def controlLoopPlanarDragging(wrench_offset, dmp, tc, controller, robot, i, past
#wrench = beta * wrench + (1 - beta) * past_data['wrench'][-1]
wrench = robot.getMtool().toDualActionMatrix().T @ wrench
- wrench = Z @ robot.getWrench()
+ wrench = Z @ wrench
# deepcopy for good coding practise (and correctness here)
save_past_dict['wrench'] = copy.deepcopy(wrench)
# rolling average
@@ -214,8 +212,6 @@ if __name__ == "__main__":
robot = RobotManager(args)
# calibrate FT first
- wrench_offset = robot.calibrateFT()
-
speed_down = np.array([0,0,-1,0,0,0])
moveUntilContact(args, robot, speed_down)
m_goal_up = robot.getMtool()
@@ -230,7 +226,7 @@ if __name__ == "__main__":
# log_dict = {
# 'wrench': np.zeros((args.max_iterations, 6)),
# }
-# controlLoop = partial(controlLoopPlanarDragging, wrench_offset, dmp, tc, controller, robot)
+# controlLoop = partial(controlLoopPlanarDragging, dmp, tc, controller, robot)
# loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
# #######################################################################
# # physical setup #
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index 78ad110f6da290dcc60383b431910c6b4154b84a..d1d06e4946e840cbb37fae117cdd8eee7bb73b74 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -5,7 +5,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
diff --git a/python/examples/test_gripper.py b/python/examples/test_gripper.py
index 366a6a583b562b14ff7c0dd4ac137ea6ee860e98..6613fdde2a794b2bbf04560270ccfbb8cfc4986d 100644
--- a/python/examples/test_gripper.py
+++ b/python/examples/test_gripper.py
@@ -6,7 +6,6 @@ import copy
import argparse
import time
from functools import partial
-from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.managers import ControlLoopManager, RobotManager
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc
index 6730f1e95d99f25500f6b1680857af2968134e4f..e6536f859f51b8b8a17c0032e2d11854161023ad 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index 26120b0cff823e6d4bc7f91a37548db23576a5ab..89732ed96ef8a84e5f69a8c8ee92ef71e3c71bee 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc
index 46c82e11fbe0ff3bb421e7f1e4c8aec6e35c9363..47dfd580761c955ca39175dd03a865d137c0bcb2 100644
Binary files a/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index c101839febbabbe17622dd0e81741568b74af6cd..b20bdc645d0677edc48f5c1d976df050f8be3c54 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -182,10 +182,9 @@ def moveUntilContactControlLoop(speed, robot, clik_controller, i, past_data):
pin.forwardKinematics(robot.model, robot.data, q)
# break if wrench is nonzero basically
wrench = robot.getWrench()
- # TODO: move this to the robot class
- wrench = wrench - robot.wrench_offset
- # TODO: remove magick number
- # it is empirical bla bla, but make it an argument at least
+ # NOTE: this 4.2 is a magic number
+ # it is a 100% empirical, with the goal being that it's just above noise.
+ # so far it's worked fine, and it's pretty soft too.
if np.linalg.norm(wrench) > 4.2:
print("hit with", np.linalg.norm(wrench))
breakFlag = True
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc
index 6d20d4dc491caffb2381d4d29f6e5fa47875ecf5..94aa9adee31a8c24a5d1149f612b7a8a46762036 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 4fba1193aab8f83f39530ea0c331f346b4b91d08..689d5b20162781539f8c9e8e0c36199d7961f5cd 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,14 +1,13 @@
-# TODO clear out unnecessary imports
# TODO rename all private variables to start with '_'
# TODO: just read the q and update everything every timestep, don't deepcopy,
+# TODO: rewrite all getSomething functions to updateSomething functions,
+# and then make the getSomething function just be return self.something.copy()
# --> just create a RobotManager.step() function, update everything there
# don't do forwardKinematics 2 extra times for no good reason. make that the libraries
# responsibility, not the users
import pinocchio as pin
import numpy as np
import time
-from pinocchio.visualize import GepettoVisualizer
-#import gepetto.corbaserver
from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
from rtde_io import RTDEIOInterface
@@ -102,8 +101,6 @@ class ControlLoopManager:
def __init__(self, robot_manager, controlLoop, args, save_past_dict, log_dict):
signal.signal(signal.SIGINT, self.stopHandler)
self.max_iterations = args.max_iterations
- # NOTE the robot manager is all over the place,
- # there might be a better design approach to that
self.robot_manager = robot_manager
self.controlLoop = controlLoop
self.args = args
@@ -115,8 +112,8 @@ class ControlLoopManager:
self.past_data[key] = deque()
# immediatelly populate every deque with initial values
for i in range(args.past_window_size):
- # deepcopy just in case, better safe than sorry.
- # and we can be slow with initialization.
+ # deepcopy just in case, better safe than sorry plus it's during initialization,
+ # not real time
self.past_data[key].append(copy.deepcopy(save_past_dict[key]))
# similar story for log_dict as for past_data,
@@ -145,7 +142,7 @@ class ControlLoopManager:
# TODO: write an assert assuring the keys are what's been promised
# (ideally this is done only once, not every time, so think whether/how that can be avoided)
for key in latest_to_save_dict:
- # remove oldes entry
+ # remove oldest entry
self.past_data[key].popleft()
# add new entry
self.past_data[key].append(latest_to_save_dict[key])
@@ -159,9 +156,6 @@ class ControlLoopManager:
#self.robot_manager.stopHandler(None, None)
self.log_dict[key][i] = log_entry_dict[key]
- #for key in log_entry_dict:
- # self.log_dict[key][i] = log_entry_dict[key]
-
# break if done
if breakFlag:
break
@@ -174,27 +168,24 @@ class ControlLoopManager:
else:
time.sleep(self.robot_manager.dt - diff)
self.final_iteration = i
-# TODO: provide a debug flag for this
-# if i < self.max_iterations -1:
-# print("success in", i, "iterations!")
-# else:
-# print("FAIL: did not succed in", max_iterations, "iterations")
-# TODO: reintroduce maybe
-# if not self.args.pinocchio_only:
-# self.robot_manager.stopHandler(None, None)
+ if args.debug_prints:
+ if i < self.max_iterations -1:
+ print("success in", i, "iterations!")
+ else:
+ print("FAIL: did not succed in", max_iterations, "iterations")
return self.log_dict, self.final_iteration
"""
stopHandler
-----------
- TODO: make ifs for simulation too
- can have self as first argument apparently.
upon receiving SIGINT it sends zeros for speed commands to
- stop the robot
+ stop the robot.
+ NOTE: apparently this isn't enough,
+ nor does stopJ do anything, so it goes to freedriveMode
+ and exists, which actually stops it.
"""
def stopHandler(self, signum, frame):
- #plotFromDict(signum, frame)
print('sending 300 speedjs full of zeros and exiting')
for i in range(300):
vel_cmd = np.zeros(6)
@@ -207,11 +198,12 @@ class ControlLoopManager:
self.robot_manager.rtde_control.freedriveMode()
plotFromDict(self.log_dict, self.final_iteration, self.args)
self.robot_manager.rtde_control.endFreedriveMode()
- #exit()
"""
robotmanager:
---------------
+- design goal: rely on pinocchio as much as possible while
+ concealing obvious bookkeeping
- right now it is assumed you're running this on UR5e so some
magic numbers are just put to it.
this will be extended once there's a need for it.
@@ -225,25 +217,14 @@ robotmanager:
actually run at 500Hz and not more.
- this is probably not the most new-user friendly solution,
but it's designed for fastest idea to implementation rate.
-- NOTE and TODO: rely on pinocchio as much as possible.
- do NOT copy data that's already present in pin.model and pin.data,
- it's just confusing. this lib already heavily relies on pinocchio,
- let's just lean into that all the way and not invent hot water.
-- TODO: write out default arguments needed
+- if this was a real programming language, all of these would be private variables.
+- TODO: write out default arguments needed here as well
"""
class RobotManager:
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
# might be changed later if that seems more appropriate
- # TODO: put the end to all modes not compatible with control
- # like freedrive or forcemode or whatever.
- # you shouldn't care about previous states
def __init__(self, args):
- # if you just stop it the program with Ctrl-C, it will continue running
- # the last speedj lmao.
- # there's also an actual stop command, but sending 0s works so i'm not changing it
- #signal.signal(signal.SIGINT, self.stopHandler)
-
self.args = args
self.pinocchio_only = args.pinocchio_only
self.simulation = args.simulation
@@ -253,58 +234,61 @@ class RobotManager:
self.model, self.collision_model, self.visual_model, self.data = \
get_model(args.visualize)
# TODO: fix this
+ # ---> TODO: write your own vedo 3D visualzier,
+ # connect it with the real-time matplotlib line plots,
+ # and add some basic gui control (show/don't show stuff,
+ # define a goal point, choose controller etc)
if args.visualize:
raise NotImplementedError('Paths in the urdf or something else need to \
be fixed to use this first. Sorry. Coming soon.')
pass
- # run visualizer from code (not working atm)
- #gepetto.corbaserver.start_server()
- #time.sleep(3)
- #viz = GepettoVisualizer(model, collision_model, visual_model)
- #viz.initViewer()
- #viz.loadViewerModel()
- #viz.display(q0)
+
# TODO: make general
if self.gripper_flag and not self.pinocchio_only:
self.gripper = RobotiqGripper()
self.gripper.connect("192.168.1.102", 63352)
self.gripper.activate()
- # TODO: this is obviously at least a partial redundancy w.r.t. the pinocchio data object
- # however i have no time to investigate what i get or don't get there
- # so i'll just leave this be. but fix this!!!! we don't want to duplicate information!!
+
# also TODO: figure out how to best solve the gripper_velocity problem
- #self.q = np.zeros(6)
- #self.qd = np.zeros(6)
- #self.qdd = np.zeros(6)
- #self.gripper_pos = 0.0
- #self.gripper_past_pos = 0.0
- # set q,qd,qdd,griper_pos, gripper_past_pos, gripper_vel to dummy values
+ # NOTE: you need to initialize differently for other types of joints
+ self.q = np.zeros(self.model.nq)
+ # v_q is the generalization of qd for every type of joint.
+ # for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
+ self.v_q = np.zeros(self.model.nv)
+ # same note as v_q, but it's a_q.
+ self.a_q = np.zeros(self.model.nv)
# initialize and connect the interfaces
self.simulation = args.simulation
- if self.pinocchio_only:
- self.q = pin.neutral(self.model)
- elif not args.simulation:
- # NOTE: you can't connect twice, dw 'bout that
+ if (not args.simulation) and (not args.pinocchio_only) :
+ # NOTE: you can't connect twice, so you can't have more than one RobotManager.
+ # if this produces errors like "already in use", and it's not already in use,
+ # try just running your new program again. it could be that the socket wasn't given
+ # back to the os even though you've shut off the previous program.
print("CONNECTING TO UR5e!")
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
self.rtde_io = RTDEIOInterface("192.168.1.102")
- if args.gripper:
+ # 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()
+ if self.gripper_flag:
self.gripper.connect("192.168.1.102", 63352)
# this is a blocking call
# this isn't actually needed and it's annoying
# TODO: test after rebooting the robot
# self.gripper.activate()
- else:
+ elif not args.pinocchio_only:
self.rtde_control = RTDEControlInterface("127.0.0.1")
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
self.rtde_io = RTDEIOInterface("127.0.0.1")
# ur specific magic numbers
+ # NOTE: all of this is ur-specific, and needs to be if-ed if other robots are added.
# TODO: this is 8 in pinocchio and that's what you actually use
# if we're being real lmao
# the TODO here is make this consistent obviously
- self.n_joints = 6
+ self.n_arm_joints = 6
# last joint because pinocchio adds base frame as 0th joint.
# and since this is unintuitive, we add the other variable too
# so that the control designer doesn't need to think about such bs
@@ -314,28 +298,23 @@ class RobotManager:
# you better not give me crazy stuff
# and i'm not clipping it, you're fixing it
assert args.acceleration <= 1.7 and args.acceleration > 0.0
- # we're not saying it's qdd because it isn't directly (apparently)
- # TODO: check that again
+ # this is the number passed to speedj
self.acceleration = args.acceleration
+ # NOTE: this is evil and everything only works if it's set to 1
+ # you really should control the acceleration via the acceleration argument.
assert args.speed_slider <= 1.0 and args.acceleration > 0.0
self.speed_slider = args.speed_slider
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
- self.max_iterations = args.max_iterations
# TODO: these are almost certainly higher
# NOTE and TODO: speed slider is evil, put it to 1, handle the rest yourself.
- # maybe you want to have them high and cap the rest with speed slider?
- # idk really, but it's better to have this low and burried for safety and robot longevity reasons
- self.max_qdd = 1.7 * args.speed_slider
- # NOTE: i had this thing at 2 in other code
+ # NOTE: i have no idea what's the relationship between max_qdd and speed slider
+ #self.max_qdd = 1.7 * args.speed_slider
+ # NOTE: this is an additional kinda evil speed limitation (by this code, not UR).
+ # we're clipping joint velocities with this.
+ # if your controllers are not what you expect, you might be commanding a very high velocity,
+ # which is clipped, resulting in unexpected movement.
self.max_qd = 0.5 * args.speed_slider
- # error limit
- # TODO this is clik specific, put it in a better place
- self.goal_error = args.goal_error
- # this is probably the right place to manage this
- self.wrench_offset = np.zeros(6)
-
-
"""
calibrateFT
@@ -354,10 +333,7 @@ class RobotManager:
print("Use this as offset.")
for i in range(2000):
start = time.time()
- q = self.rtde_receive.getActualQ()
ft = self.rtde_receive.getActualTCPForce()
- tau = self.rtde_control.getJointTorques()
- current = self.rtde_receive.getActualCurrent()
ft_readings.append(ft)
end = time.time()
diff = end - start
@@ -365,10 +341,9 @@ class RobotManager:
time.sleep(self.dt - diff)
ft_readings = np.array(ft_readings)
- avg = np.average(ft_readings, axis=0)
- self.wrench_offset = avg
- print("average ft time", avg)
- return avg
+ self.wrench_offset = np.average(ft_readings, axis=0)
+ print(self.wrench_offset)
+ return self.wrench_offset.copy()
"""
step
@@ -395,9 +370,14 @@ class RobotManager:
def step(self):
self.getQ()
self.getQd()
- self.wrench = np.array(self.rtde_receive.getActualTCPForce())
- pin.forwardKinematics(self.model, self.data, self.q)
- self.Mtool = self.data.oMi[self.JOINT_ID]
+ self.getWrench()
+ # certainly not necessary btw
+ # but if it runs on time, does it matter? it makes everything available...
+ pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
+ self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+ # this isn't real because we're on a velocity-controlled robot,
+ # so this is actually None (no tau, no a_q, as expected)
+ self.a_q = self.data.ddq
"""
setSpeedSlider
@@ -488,8 +468,8 @@ class RobotManager:
self.q = q
pin.forwardKinematics(self.model, self.data, q)
# TODO probably remove deepcopy
- self.Mtool = self.data.oMi[self.JOINT_ID]
- return copy.deepcopy(self.Mtool)
+ self.T_w_e = self.data.oMi[self.JOINT_ID]
+ return self.T_w_e.copy()
"""
getQd
@@ -522,11 +502,8 @@ class RobotManager:
# let's just have both options for getting q, it's just a 8d float list
# readability is a somewhat subjective quality after all
qd = np.array(qd)
- self.qd = qd
- # TODO make sure this makes sense
- else:
- return self.qd
- return qd
+ self.v_q = qd
+ return self.v_q.copy()
"""
getWrench
@@ -536,14 +513,17 @@ class RobotManager:
here because this things depend on the arguments which are manager here (hence the
class name RobotManager)
"""
- def getWrench(self):
+ def getWrenchRaw(self):
if not self.pinocchio_only:
wrench = np.array(self.rtde_receive.getActualTCPForce())
else:
raise NotImplementedError("Don't have time to implement this right now.")
-
return wrench
+ def getWrench(self):
+ self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
+ return self.wrench.copy()
+
"""
sendQd
-----
@@ -584,7 +564,7 @@ class RobotManager:
q = self.getQ()
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))
- Mtool = self.data.oMi[self.JOINT_ID]
+ T_w_e = self.data.oMi[self.JOINT_ID]
if not self.args.visualize:
print("You can only specify the translation right now.")
if not self.pinocchio_only:
@@ -596,7 +576,7 @@ class RobotManager:
# remain with the current orientation
# TODO: add something, probably rpy for orientation because it's the least number
# of numbers you need to type in
- Mgoal = copy.deepcopy(Mtool)
+ Mgoal = T_w_e.copy()
# this is a reasonable way to do it too, maybe implement it later
#Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
# do a while loop until this is parsed correctly
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc
index 0b438269c81061782619270835f25ca18b5845e3..310d0f3ccd9af60dab2a7927182652fe8b3decea 100644
Binary files a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc
index d70d22745d75183a65543fa1b8c7a0c8298cd164..87cf9af8fa0a9f776b63d90be581eb2a06f08dde 100644
Binary files a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc
index cc7e2ef70b43104b84abbcf2d7964c31e18b8618..73ad86e2073753e1f862d0c21f332c78f73adcef 100644
Binary files a/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 4f5e2990e6644b46ae0420a9cac8ef5da6c2e1ed..386fce4af890b63f03d87eb47a3d28d507c0012d 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc
index fd1cbb042ac4e18c03f4548f12b5ef3c475a0fd8..480d94bcf9e5d9e5d77bb06fefa9fe15d9971f79 100644
Binary files a/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/default_args.py b/python/ur_simple_control/util/default_args.py
index aea31eada73c866974221c443644217a068a2cb4..218de8545f1b2c8b13ef45653f48418ac1633911 100644
--- a/python/ur_simple_control/util/default_args.py
+++ b/python/ur_simple_control/util/default_args.py
@@ -4,130 +4,11 @@ import argparse
"""
-preferably you copy-paste these parameters and add/remove
-arguments to suit your specific arguments.
-the generic arguments are pretty much always used.
-TODO: think about setting the generic arguments somewhere else,
-not parsing there, but returning the parses and then only adding
-stuff you want.
-that way it's a bit cleanear, and you also can't delete the obligatory ones
+TODO: have 2 versions:
+ 1. only the minimal set of argumets (those needed to load robot
+ and run anything)
+ 2. every single one you have os far
"""
def getDefaultArgs():
- #######################################################################
- # generic arguments #
- #######################################################################
- parser = argparse.ArgumentParser(description='Make a drawing on screen,\
- watch the robot do it on the whiteboard.',
- formatter_class=argparse.ArgumentDefaultsHelpFormatter)
- # TODO this one won't really work but let's leave it here for the future
- parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
- help="whether you are running the UR simulator. \
- NOTE: doesn't actually work because it's not a physics simulator", \
- default=False)
- parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
- help="whether you want to just integrate with pinocchio.\
- NOTE: doesn't actually work because it's not a physics simulator", \
- default=False)
- parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
- help="whether you want to visualize with gepetto, but \
- NOTE: not implemented yet", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
- help="whether you're using the gripper", default=False)
- parser.add_argument('--acceleration', type=float, \
- help="robot's joints acceleration. scalar positive constant, \
- max 1.7, and default 0.4. \
- BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
- TODO: check what this means", default=0.3)
- parser.add_argument('--speed-slider', type=float,\
- help="cap robot's speed with the speed slider \
- to something between 0 and 1, 1.0 by default because for dmp. \
- BE CAREFUL WITH THIS.", default=1.0)
- parser.add_argument('--max-iterations', type=int, \
- help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
- #######################################################################
- # your controller specific arguments #
- #######################################################################
- # not applicable here, but leaving it in the case it becomes applicable
- # it's also in the robot manager even though it shouldn't be
- parser.add_argument('--past-window-size', type=int, \
- help="how many timesteps of past data you want to save", default=5)
- parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with. NOTE: not used here", \
- default=1e-3)
- # TODO: test the interaction of this and the overall demo
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tiknohov regularization.\
- This is used when generating the joint trajectory from the drawing.", \
- default=1e-2)
- # TODO add the rest
- parser.add_argument('--clik-controller', type=str, \
- help="select which click algorithm you want", \
- default='dampedPseudoinverse', \
- choices=['dampedPseudoinverse', 'jacobianTranspose'])
- # maybe you want to scale the control signal
- parser.add_argument('--controller-speed-scaling', type=float, \
- default='1.0', help='not actually_used atm')
- #############################
- # dmp specific arguments #
- #############################
- parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
- help="whether you want to use temporal coupling", default=True)
- parser.add_argument('--kp', type=float, \
- help="proportial control constant for position errors", \
- default=1.0)
- parser.add_argument('--tau0', type=float, \
- help="total time needed for trajectory. if you use temporal coupling,\
- you can still follow the path even if it's too fast", \
- default=5)
- parser.add_argument('--gamma-nominal', type=float, \
- help="positive constant for tuning temporal coupling: the higher,\
- the fast the return rate to nominal tau", \
- default=1.0)
- parser.add_argument('--gamma-a', type=float, \
- help="positive constant for tuning temporal coupling, potential term", \
- default=0.5)
- parser.add_argument('--eps-tc', type=float, \
- help="temporal coupling term, should be small", \
- default=0.001)
- parser.add_argument('--alpha', type=float, \
- help="force feedback proportional coefficient", \
- default=0.007)
- # TODO add low pass filtering and make it's parameters arguments too
- #######################################################################
- # task specific arguments #
- #######################################################################
- # TODO measure this for the new board
- parser.add_argument('--board-width', type=float, \
- help="width of the board (in meters) the robot will write on", \
- default=0.5)
- parser.add_argument('--board-height', type=float, \
- help="height of the board (in meters) the robot will write on", \
- default=0.35)
- parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
- help="whether you want to do calibration", default=False)
- parser.add_argument('--draw-new', action=argparse.BooleanOptionalAction, \
- help="whether draw a new picture, or use the saved path path_in_pixels.csv", default=True)
- parser.add_argument('--pick_up_marker', action=argparse.BooleanOptionalAction, \
- help="""
- whether the robot should pick up the marker.
- NOTE: THIS IS FROM A PREDEFINED LOCATION.
- """, default=True)
- parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \
- help="""
- whether you want to do find marker offset (recalculate TCP
- based on the marker""", default=False)
- parser.add_argument('--n-calibration-tests', type=int, \
- help="number of calibration tests you want to run", default=10)
- parser.add_argument('--clik-goal-error', type=float, \
- help="the clik error you are happy with", default=1e-2)
- parser.add_argument('--max-init-clik-iterations', type=int, \
- help="number of max clik iterations to get to the first point", default=10000)
- parser.add_argument('--max-running-clik-iterations', type=int, \
- help="number of max clik iterations between path points", default=1000)
-
- args = parser.parse_args()
- if args.gripper and args.simulation:
- raise NotImplementedError('Did not figure out how to put the gripper in \
- the simulation yet, sorry :/ . You can have only 1 these flags right now')
- return args
+ raise NotImplementedError("sorry, this hasn't been implemented yet")
diff --git a/python/ur_simple_control/util/draw_path.py b/python/ur_simple_control/util/draw_path.py
index 0fa950d844b57782611bfcf0dcb3b19552cb1311..4a54dca26f391b847aa8811f09a76e632748e387 100644
--- a/python/ur_simple_control/util/draw_path.py
+++ b/python/ur_simple_control/util/draw_path.py
@@ -1,10 +1,14 @@
-# TODO: possible improvement:
-# - draw multiple lines
-# - then you would just generate multiple dmps for each trajectory
-# and do movel's + movej's to provide the connections
-# TODO: possible improvement: make it all bezier curves
-# https://matplotlib.org/stable/users/explain/artists/paths.html
-# look at the example for path handling if that's what you'll need
+"""
+possible improvement:
+- draw multiple lines
+- then you would just generate multiple dmps for each trajectory
+ and do movel's + movej's to provide the connections
+possible improvement: make it all bezier curves
+ https://matplotlib.org/stable/users/explain/artists/paths.html
+ look at the example for path handling if that's what you'll need
+ - not really needed, especially because we want hard edges to test our controllers
+ (but if that was a parameter that would be ok i guess)
+"""
import numpy as np
import matplotlib.pyplot as plt
@@ -21,9 +25,10 @@ from matplotlib.widgets import LassoSelector
#from matplotlib.path import Path
class DrawPathManager:
- def __init__(self, ax):
+ def __init__(self, args, ax):
self.canvas = ax.figure.canvas
self.lasso = LassoSelector(ax, onselect=self.onselect)
+ self.args = args
def onselect(self, verts):
# verts is a list of tuples
@@ -40,16 +45,15 @@ class DrawPathManager:
# made to save and exit
def accept(self, event):
if event.key == "enter":
- print("pixel path:")
- print(self.path)
+ if args.debug_prints:
+ print("pixel path:")
+ print(self.path)
self.disconnect()
- # TODO: ensure this runs fine after packaging
np.savetxt("./path_in_pixels.csv", self.path, delimiter=',', fmt='%.5f')
- print("TODO: run clik on the pixel path to make it a trajectory")
# plt.close over exit so that we can call this from elsewhere and not kill the program
plt.close()
-def drawPath():
+def drawPath(args):
# normalize both x and y to 0-1 range
# we can multiply however we want later
# idk about the number of points, but it's large enough to draw
@@ -59,7 +63,7 @@ def drawPath():
subplot_kw = dict(xlim=(0, 1), ylim=(0, 1), autoscale_on=False)
fig, ax = plt.subplots(subplot_kw=subplot_kw)
- selector = DrawPathManager(ax)
+ selector = DrawPathManager(args, ax)
# map key press to our function
# thankfully it didn't mind have self as the first argument
@@ -70,4 +74,4 @@ def drawPath():
if __name__ == '__main__':
- drawPath()
+ drawPath(args)
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 4a32e98a4303921a0fc9e50ea8ce51f2105ea85f..3059057a447fc0a92c6d2b5b509cc12f88b0eeb2 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -1,3 +1,9 @@
+"""
+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
@@ -6,7 +12,6 @@ from importlib.resources import files
# can't get the urdf reading with these functions to save my life, idk what or why
-# TODO fix paths via python packaging
def get_model(visualize):
#urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 730ff2c23de6775015e64007f0de2156f621bd57..c8f0c5d516ae35675b8624b16abef47998bc452b 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -6,8 +6,10 @@ def saveLog(log_dict, final_iteration, args):
# shave off the zeros, noone needs 'em
for key in log_dict:
log_dict[key] = log_dict[key][:final_iteration]
- # TODO make generic
- # preferably name files after arguments
+ # TODO make generic:
+ # - generate name based on args + add index
+ # - you need to run shell ls to get the index,
+ # there's at least one chalmers project with code for that
run_file_path = "./data/clik_run_001.pickle"
args_file_path = "./data/clik_run_001_args.pickle"
# save the logged data
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc
index aba675302559b05bdf8aa8c563b486bbf526fd1f..ac4eb3273e2b46597521f26cb6adac319d37d244 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d3f8454db4cce95335a702b67ae9e76fa717ce46
Binary files /dev/null and b/python/ur_simple_control/visualize/__pycache__/pin_to_vedo_plot_util.cpython-311.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc
index 1b8fab368fc8d0086e4066c6ee5f220f2bd77329..44f507b6f1ebdeda62574ea70174ab2afce3bc34 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc differ
diff --git a/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py b/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py
index d400cf7f74ecd176a77ab8ecf0dfca5e27a8a500..a89138777224afd73d6ea4369234e6cbe43d0034 100644
--- a/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py
+++ b/python/ur_simple_control/visualize/pin_to_vedo_plot_util.py
@@ -36,13 +36,12 @@ def updateFrameArrowsFromSE3(arrows : list[vedo.Arrow], frame : pin.SE3):
arrows[i].top = np.array([x,y,z])
def drawSE3AsFrame(frame : pin.SE3,
- text : str) -> vedo.Arrows:
+ text : str, scaling=0.1) -> vedo.Arrows:
#print(text)
#print(frame)
colors = ['r', 'g', 'b']
arrows = []
#print(frame.rotation)
- scaling = 0.02
for i in range(0,3):
x = frame.translation[0] + frame.rotation[0,i] * scaling
y = frame.translation[1] + frame.rotation[1,i] * scaling
diff --git a/python/ur_simple_control/visualize/vedo_manipulator.py b/python/ur_simple_control/visualize/vedo_manipulator.py
index 5c3972b701ee4da0621447c0d9847a10755f727c..cf03cc89f9d885bae5db99efafd66c8bcb833367 100644
--- a/python/ur_simple_control/visualize/vedo_manipulator.py
+++ b/python/ur_simple_control/visualize/vedo_manipulator.py
@@ -15,222 +15,95 @@ import vedo
import pinocchio as pin
import time
from ur_simple_control.visualize.pin_to_vedo_plot_util import *
-from friction import ReducedFrictionModel
-from multiprocessing import Process
-from scipy.integrate import solve_ivp
-
-
+from ur_simple_control.managers import RobotManager
+import argparse
+
+
+def get_args():
+ parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!',
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+ parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
+ help="whether you are running the UR simulator", default=False)
+ parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
+ help="whether you want to just integrate with pinocchio", default=True)
+ parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize with gepetto, but NOTE: not implemented yet", default=False)
+ parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
+ help="whether you're using the gripper", default=False)
+ parser.add_argument('--goal-error', type=float, \
+ help="the final position error you are happy with", default=1e-2)
+ parser.add_argument('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ parser.add_argument('--acceleration', type=float, \
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means", default=0.3)
+ 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=0.5)
+ parser.add_argument('--tikhonov-damp', type=float, \
+ help="damping scalar in tiknohov regularization", default=1e-3)
+ # TODO add the rest
+ parser.add_argument('--clik-controller', type=str, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
+ # maybe you want to scale the control signal
+ parser.add_argument('--controller-speed-scaling', type=float, \
+ default='1.0', help='not actually_used atm')
+ parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
+ help="print some debug info", default=False)
+
+ args = parser.parse_args()
+ if args.gripper and args.simulation:
+ raise NotImplementedError('Did not figure out how to put the gripper in \
+ the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ return args
#######################################################################
# IMPLEMENT THIS ACTUALLY #
#######################################################################
-if __name__ == "__main__":
- args = getArgs()
- # change this after you add the manipulator
- box_dimensions = [args.box_length, args.box_width, args.box_height]
-
- ###########################################
- # vedo instantiation, frame definitions #
- ###########################################
- vedo.settings.use_depth_peeling = True
-
- # need to manually seed lmao
- #pin.seed(int(time.time()))
- T_w_J = pin.SE3()
- # TODO: check how friction behaves when we're perpendicular to gravity
- T_w_J.setIdentity()
- T_w_J.translation = np.array([1,2,3])
- T_w_J.setRandom()
- #T_w_J.rotation = pin.rpy.rpyToMatrix(0,np.pi/2,0)
-
-
- model, data, OBJECT_JOINT_ID = getPlanarJointModel(args, T_w_J, box_dimensions)
- q_init = pin.randomConfiguration(model)
- #theta = np.random.random() * 2 * np.pi
- theta = 0.345321
- q_init[0] = 0.1
- q_init[1] = 0.1
- q_init[2] = np.cos(theta)
- q_init[3] = np.sin(theta)
-
- v_b_se2 = np.array([0.1, 0.0, 0.5])
- v_b_se3 = pin.Motion(np.array([v_b_se2[0], v_b_se2[1], 0]), np.array([0,0,v_b_se2[2]]))
- print("v_b_se2", v_b_se2)
- #print("pin.exp3(v)", pin.exp3(v))
- #exit()
- #tau = np.ones((model.nv)) * 0.1
- #a = pin.aba(model, data, q, v, tau)
- # tau_q_control does not exist in reality, those are the actuators
- # of the planar joint
- #tau_q_control = np.zeros((model.nv))
-
- pin.forwardKinematics(model, data, q_init)
- T_w_b_init = data.oMi[1].copy()
-
- q_pin = pin.integrate(model, q_init, v_b_se2)
- print("q_pin:", q_pin)
- pin.forwardKinematics(model, data, q_pin)
- pin.updateFramePlacements(model, data)
-# for vvv in data.v:
-# print(vvv)
-# for fr in data.oMf:
-# print(fr)
- T_w_b_pin = data.oMi[1].copy()
-
- q_3 = np.zeros(3)
- q_3[0] = q_init[0]
- q_3[1] = q_init[1]
- q_3[2] = theta
- print("q_3", q_3)
-
- in_SE3 = False
-
-########################################################
-## JUST USE THE EXP FOR THE LOVE OF GOD
-#############################################3333333
-# omega = v[2]
-# cv = np.cos(omega)
-# sv = np.sin(omega)
-# R_omega = np.array([[cv, -1*sv], [sv, cv]])
-# vcross = np.array([-1*v[1], v[0]])
-# vcross -= -v[:2]*R_omega[:,0] + v[:2]*R_omega[:,1]
-# vcross /= omega
-# t = vcross.copy()
-# T_next = np.hstack((R_omega, t.reshape((2,1))))
-# T_next = np.vstack((T_next, np.array([0,0,1]).reshape((1,3))))
-# # let's try inverse
-# #T_next = np.hstack((R_omega.T, -1*R_omega.T @ t.reshape((2,1))))
-# #T_next = np.vstack((T_next, np.array([0,0,1]).reshape((1,3))))
-# print("T_next", T_next)
-
-# c_theta = np.cos(theta)
-# s_theta = np.sin(theta)
-# R_theta = np.array([[c_theta, -1*s_theta], [s_theta, c_theta]])
-# T_J_b_se2 = np.hstack((R_theta, np.array([q_3[0],q_3[1]]).reshape((2,1))))
-# T_J_b_se2 = np.vstack((T_J_b_se2, np.array([0,0,1]).reshape((1,3))))
-
-# print("T_J_b_se2", T_J_b_se2)
-# T_J_b_se2 = T_next @ T_J_b_se2
-# print("T_J_b_se2", T_J_b_se2)
-
-# T_next_SE3 = pin.exp(v_b_se3)
-# T_next_SE2 = np.hstack((T_next_SE3.rotation[:2,:2], T_next_SE3.translation[:2].reshape((2,1))))
-# T_next_SE2 = np.vstack((T_next_SE2, np.array([0,0,1]).reshape((1,3))))
-
- # wrogn notation last term lel
-# T_J_b_SE2 = T_next_SE2 @ T_J_b_se2
-# q_3[0] = T_J_b_SE2[0,2]
-# q_3[1] = T_J_b_SE2[1,2]
-# # not good but w/3
-# q_3[2] = np.arccos(T_J_b_SE2[0,0])
-
- # this changes the no rotation problem
- #q_3[2] = omega
+def drawStateAnim(self):
+ toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])]
+ drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)), self.avg_link_lenth * 2)
+ for j in range(len(self.joints)):
+ toBase.append(toBase[-1] @ self.joints[j].HomMat)
+ drawOrientationAnim(self.ax, toBase[-1][0:3,0:3], self.lines[j][:-1], toBase[-1][0:3,3], self.avg_link_lenth)
+ drawVectorAnim(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), self.lines[j][-1], toBase[-2][0:3,3],self.color_link)
+
+
+if __name__ == "__main__":
+ pin.seed(int(time.time()))
+ args = get_args()
+ robot = RobotManager(args)
+ q = pin.randomConfiguration(robot.model)
+ q = np.zeros(8)
+ pin.forwardKinematics(robot.model, robot.data, q)
+ print(type(robot.data))
+ #pin.computeAllTerms(robot.model, robot.data, q, np.zeros(8))
+ # no need to use AnimationPlayer, we can update stuff on Plotter as we please.
+ # so the comm logic + updating will be implemented here (ideal circumstance)
+ plt = vedo.Plotter(axes=1)
+ plt.camera = vedo.utils.oriented_camera(center=(0, 0, 0),
+ up_vector=(0, 1, 0),
+ backoff_vector=(0, 1, 0),
+ backoff=1.0)
- #print("q_3 + v_b_se2", q_3)
-#################
-# q_euler = np.zeros(4)
-# q_euler[0] = q_3[0] #+ np.cos(v[2])
-# q_euler[1] = q_3[1] #+ np.exp(v[2])
-# q_euler[2] = np.cos(q_3[2])
-# q_euler[3] = np.sin(q_3[2])
-# print("q_euler:", q_euler)
-# pin.forwardKinematics(model, data, q_euler)
- #T_w_b_euler = data.oMi[1].copy()
- #T_w_b_euler = pin.exp6(v_b_se3).act(T_w_J)
- #T_w_b_euler = pin.exp6(v_twist).act(T_w_b_init)
-# T_J_b = T_w_J.actInv(T_w_b_init)
- #print(T_J_b)
- #T_w_b_euler = pin.exp6(T_J_b.act(v_twist)).act(T_w_b_init)
- # THIS IS THE ONE!!!!!!!!!!!!!!!!!!!
- if in_SE3:
- print("SE3 version")
- v_w_se3 = T_w_b_init.act(v_b_se3)
- print(v_w_se3)
- T_w_next_SE3 = pin.exp6(v_w_se3)
- print(T_w_next_SE3)
- T_w_b_euler = T_w_next_SE3.act(T_w_b_init)
- #T_w_b_euler = pin.exp6(v_twist).act(T_w_b_init)
- #T_J_b_next_euler = pin.exp6(v_twist).act(T_J_b)
- #T_w_b_euler = T_w_J.act(T_J_b_next_euler)
- # let's try to get this within J and SE(2)
- # get v_b to v_J
- else:
- print("SE2 version")
- c_theta = np.cos(theta)
- s_theta = np.sin(theta)
- R_theta = np.array([[c_theta, -1*s_theta], [s_theta, c_theta]])
- # needed later
- T_J_b_SE2 = np.zeros((3,3))
- T_J_b_SE2[:2,:2] = R_theta
- T_J_b_SE2[0,2] = q_3[0]
- T_J_b_SE2[1,2] = q_3[1]
- T_J_b_SE2[2,2] = 1
- Ad_T_J_b_SE2 = np.zeros((3,3))
- Ad_T_J_b_SE2[:2,:2] = R_theta
- Ad_T_J_b_SE2[0,2] = q_init[1]
- Ad_T_J_b_SE2[1,2] = -q_init[0]
- Ad_T_J_b_SE2[2,2] = 1
- v_J_se2 = Ad_T_J_b_SE2 @ v_b_se2
- print("v_J_se2", v_J_se2)
- # get v_J_se2 to T_next_SE2
- omega = v_J_se2[2]
- c_omega = np.cos(omega)
- s_omega = np.sin(omega)
- # note that this is the same as exp(omega)
- R_omega = np.array([[c_omega, -1*s_omega], [s_omega, c_omega]])
- omega_abs = np.abs(omega)
- t = np.zeros(2)
- if omega_abs > 1e-14:
- V = np.array([[s_omega, -1*(1 -c_omega)], [1- c_omega, s_omega]])
- V /= omega
- t = V @ np.array([v_J_se2[0], v_J_se2[1]])#.reshape((2,1))
- else:
- t = np.zeros(2)
- t[0] = v_J_se2[0]
- t[1] = v_J_se2[1]
- T_next_SE2 = np.hstack((R_omega, t.reshape((2,1))))
- T_next_SE2 = np.vstack((T_next_SE2, np.array([0,0,1]).reshape((1,3))))
- print("T_next_SE2", T_next_SE2)
- T_J_b_SE2 = T_next_SE2 @ T_J_b_SE2
- q_euler = np.zeros(4)
- q_euler[0] = T_J_b_SE2[0,2]
- q_euler[1] = T_J_b_SE2[1,2]
- q_euler[2] = T_J_b_SE2[0,0]
- q_euler[3] = T_J_b_SE2[1,0]
- #q_euler[0] = q_3[0]
- #q_euler[1] = q_3[1]
- #q_euler[2] = np.cos(q_3[2])
- #q_euler[3] = np.sin(q_3[2])
- print("q3 + v_J", q_3 + v_J_se2)
- print("q_euler:", q_euler)
- pin.forwardKinematics(model, data, q_euler)
- T_w_b_euler = data.oMi[OBJECT_JOINT_ID]
+ for i in range(robot.model.nq):
+ plt += drawSE3AsFrame(robot.data.oMi[i], "",scaling=0.02)
+ if i+1 < robot.model.nq:
+ # plt += vedo.Arrow(robot.data.oMi[i].translation,
+ # robot.data.oMi[i+1].translation - robot.data.oMi[i].translation)
+ #plt += vedo.Arrow(robot.data.oMi[i].translation,
+ # -1 * (robot.data.oMi[i].translation - robot.data.oMi[i+1].translation))
+ plt += vedo.Arrow(robot.data.oMi[i].translation,
+ robot.data.oMi[i].act(robot.data.liMi[i+1]).translation)
- joint_arrows, joint_text = drawSE3AsFrame(T_w_J, "J")
- b_init_arrows, b_init_text = drawSE3AsFrame(T_w_b_init, "b_init")
- # pin
- objectt = drawBoxFromSE3(T_w_b_pin, box_dimensions)
- b_arrows_pin, b_text_pin = drawSE3AsFrame(T_w_b_pin, "b_pin")
- # euler
- objectt = drawBoxFromSE3(T_w_b_pin, box_dimensions)
- b_arrows_euler, b_text_euler = drawSE3AsFrame(T_w_b_euler, "b_euler")
- plt = vedo.Plotter(axes=1)
- plt.camera = vedo.utils.oriented_camera(center=(0, 0, 0),
- up_vector=(0, 1, 0),
- backoff_vector=(0, 1, 0),
- backoff=1.0)
-
- plt.add(objectt)
- plt.add(b_init_arrows, b_init_text)
- plt.add(b_arrows_pin, b_text_pin)
- plt.add(b_arrows_euler, b_text_euler)
- plt.add(joint_arrows, joint_text)
plt.render()
plt.show()
- plt.close()