diff --git a/python/convenience_tool_box/.measuring_stick.py.swp b/python/.setup.py.swp
similarity index 84%
rename from python/convenience_tool_box/.measuring_stick.py.swp
rename to python/.setup.py.swp
index 565f0d5acdeb8db8100b7857fee177e575e0ce70..fc1c48cd886b09bab7b4826d832c3761456cf528 100644
Binary files a/python/convenience_tool_box/.measuring_stick.py.swp and b/python/.setup.py.swp differ
diff --git a/python/README.md b/python/README.md
index ca8d7091deae9b3e0d030e22c529a3c66950cd9a..0368c0812f33c799aa5d67c3fa70518f7f033fb9 100644
--- a/python/README.md
+++ b/python/README.md
@@ -1,6 +1,10 @@
# TODO:
---------
- go to a previous version, save basic version of click where everything is in one file
+- write test for every algorithm - just run it on sample data on simulation and pinocchio.
+ using things on the real robot needs to be tested manually, but simulation in URsimulator should
+ take care of that because the only difference is the IP address in that case.
+ point is test what you can.
# installation
------------
diff --git a/python/setup.py b/python/setup.py
index cbd9fa92b7565c362b28909acddb0d5b76cf559d..9daa358d1cec03040a37041898ffc18c66243290 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -1,10 +1,24 @@
from setuptools import setup, find_packages
setup(name='ur_simple_control',
- version='0.0.1',
- # TODO add other ones and test, check .toml file for more
- install_requires=['numpy'],
- packages=find_packages(where='src'),
-
- package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
- )
+ version='0.1',
+ description='Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.',
+ author='Marko Guberina',
+ url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
+ packages=['ur_simple_control'],
+ # package_dir={"": "ur_simple_control"},
+ package_data={
+ 'ur_simple_control.robot_descriptions': ['*'],
+ },
+ zip_safe=False)
+# NOTE: if you want to switch to the toml thing,
+# here you go, knock yourself out, didn't really work for me,
+# and i don't care at this stage
+ # add other ones and test, check .toml file for more
+ # dependencies need to be taken care of separately
+ # because you don't want to install pinocchio via pip
+ #install_requires=['numpy'],
+ # broken, but if you'll want to switch later here you go
+ #packages=find_packages(where='src'),
+ #package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
+ #)
diff --git a/python/ur_simple_control.egg-info/PKG-INFO b/python/ur_simple_control.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..b15b402cdc66b7291a2d30d186c855ea27331755
--- /dev/null
+++ b/python/ur_simple_control.egg-info/PKG-INFO
@@ -0,0 +1,7 @@
+Metadata-Version: 2.1
+Name: ur-simple-control
+Version: 0.1
+Summary: Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.
+Home-page: https://gitlab.control.lth.se/marko-g/ur_simple_control
+Author: Marko Guberina
+License-File: LICENSE.txt
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..77d698460a7839b89310d3705ce6a4b5496d5b4e
--- /dev/null
+++ b/python/ur_simple_control.egg-info/SOURCES.txt
@@ -0,0 +1,9 @@
+LICENSE.txt
+README.md
+setup.py
+ur_simple_control/__init__.py
+ur_simple_control.egg-info/PKG-INFO
+ur_simple_control.egg-info/SOURCES.txt
+ur_simple_control.egg-info/dependency_links.txt
+ur_simple_control.egg-info/not-zip-safe
+ur_simple_control.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/ur_simple_control.egg-info/dependency_links.txt b/python/ur_simple_control.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control.egg-info/not-zip-safe b/python/ur_simple_control.egg-info/not-zip-safe
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control.egg-info/not-zip-safe
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control.egg-info/top_level.txt b/python/ur_simple_control.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9a3fac693b97cd420a40e3e0cb5d493d31b30333
--- /dev/null
+++ b/python/ur_simple_control.egg-info/top_level.txt
@@ -0,0 +1 @@
+ur_simple_control
diff --git a/python/ur_simple_control/__init__.py b/python/ur_simple_control/__init__.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..38c21a51e1c542c6d8e3da9be01d84d24364e066 100644
--- a/python/ur_simple_control/__init__.py
+++ b/python/ur_simple_control/__init__.py
@@ -0,0 +1,4 @@
+from ur_simple_control import clik, dmp, robot_descriptions, util
+__all__ = [
+"clik", "dmp", "robot_descriptions", "util",
+]
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index 0564b9f600679f04e19e0738fa67cd4f9fecb063..f0e52da6259b072f7fdf395595c1bd877a6aea0b 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
index 7e2d03a88b0a8fd17bdc30aa40008b6dee547ff7..9fc488300be91c4a2b8a7a31043e23c54fcc35ba 100644
Binary files a/python/ur_simple_control/clik/.clik.py.swp and b/python/ur_simple_control/clik/.clik.py.swp differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..22ff463a3a86e96cac507291831172989a59a7cc
Binary files /dev/null and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 7f40d33c8673376eaf071c653fbb2ea878f8f92f..6f100662fe064999c940ba28634979336120f585 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -75,7 +75,7 @@ TODO: write out other algorithms
"""
def getController(args):
if args.controller == "dampedPseudoinverse":
- return partial(dampedPseudoinverse, args.tiknonov_damp)
+ return partial(dampedPseudoinverse, args.tikhonov_damp)
if args.controller == "jacobianTranspose":
return jacobianTranspose
# TODO implement and add in the rest
@@ -107,11 +107,11 @@ def controlLoopClik(robot, controller, i):
# first check whether we're at the goal
SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
+ if np.linalg.norm(err_vector) < robot.goal_error:
print("Convergence achieved, reached destionation!")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
# compute the joint velocities.
# just plug and play different ones
qd = controller(J, err_vector)
@@ -119,7 +119,7 @@ def controlLoopClik(robot, controller, i):
# we do the printing here because controlLoopManager should be general.
# and these prints are click specific (although i'm not 100% happy with the arrangement)
if not i % 1000:
- print("pos", data.oMi[JOINT_ID])
+ print("pos", robot.data.oMi[robot.JOINT_ID])
print("linear error = ", pin.log6(SEerror).linear)
print("angular error = ", pin.log6(SEerror).angular)
print(" error = ", err_vector.transpose())
@@ -133,5 +133,5 @@ if __name__ == "__main__":
Mgoal = robot.defineGoalPoint()
controller = getController(args)
controlLoop = partial(controlLoopClik, robot, controller)
- controlLoopManager(controlLoop, args)
- controlLoopManager.run()
+ loop_manager = ControlLoopManager(controlLoop, args)
+ loop_manager.run()
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1935145d6fced2a08057f336744c1f3174952a6e
Binary files /dev/null and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/robot_descriptions/__init__.py b/python/ur_simple_control/robot_descriptions/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3c8fc8dc0aef0b7824804dd26c62c026082113cd
Binary files /dev/null and b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/robot_descriptions/config/__init__.py b/python/ur_simple_control/robot_descriptions/config/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/config/ur5e/__init__.py b/python/ur_simple_control/robot_descriptions/config/ur5e/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/meshes/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/meshes/ur5e/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/ur5e/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/meshes/ur5e/collision/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/ur5e/collision/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/meshes/ur5e/visual/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/ur5e/visual/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__init__.py b/python/ur_simple_control/robot_descriptions/urdf/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..604494c47ace559fed14c143479ef69e2f44abc8
Binary files /dev/null and b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
index d0befeb34512b3259ffe76f42513039b4404548a..30b86a09891ea955ae68c488d3e981f4f26b79da 100644
Binary files a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp and b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp differ
diff --git a/python/ur_simple_control/util/.get_model.py.swp b/python/ur_simple_control/util/.get_model.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..bd148d33717e53ec67b40270ae0735a98b999d35
Binary files /dev/null and b/python/ur_simple_control/util/.get_model.py.swp differ
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..41c97b7686410f2feed21cab462d3e8c926d6c9c
Binary files /dev/null and b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..8b3e26aeb2ede980f7ca8364b51c2f398c05e7c0
Binary files /dev/null and b/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..184950c3ec3f73f88e80199f281657e18a3678c9
Binary files /dev/null and b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9310cab2c4819c34952aa62d53df3b15536a6460
Binary files /dev/null and b/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/util/boilerplate_wrapper.py
index 99e0492d741f8c29f28b91c3f3d0308b1ca78474..e93b62d75c830045d473b22d2514402a6acb48e8 100644
--- a/python/ur_simple_control/util/boilerplate_wrapper.py
+++ b/python/ur_simple_control/util/boilerplate_wrapper.py
@@ -7,7 +7,7 @@ from pinocchio.visualize import GepettoVisualizer
from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import copy
import signal
# TODO make the package work
@@ -25,10 +25,14 @@ like max_iterations, what data to save etc.
NOTE: you give this the ready-made control loop.
if it has arguments, bake them in with functools.partial.
"""
-class ControlLoopManager(controlLoop, args):
- def __init__(self, controlLoop, max_iterations):
+class ControlLoopManager:
+ def __init__(self, controlLoop, args):
self.max_iterations = args.max_iterations
self.controlLoop = controlLoop
+ self.args = args
+ # predefined ur magic numbers
+ self.update_rate = 500
+ self.dt = 1 / self.update_rate
# if you just stop it the program with Ctrl-C, it will continue running
# the last speedj lmao.
@@ -76,16 +80,17 @@ class ControlLoopManager(controlLoop, args):
break
end = time.time()
diff = end - start
- if dt < diff:
- print("missed deadline by", diff - dt)
+ if self.dt < diff:
+ print("missed deadline by", diff - self.dt)
continue
else:
- time.sleep(dt - diff)
+ time.sleep(self.dt - diff)
if i < self.max_iterations -1:
print("success in", i, "iterations!")
else:
- print("FAIL: did not succed in," max_iterations, "iterations")
- self.stopHandler(None, None)
+ print("FAIL: did not succed in", max_iterations, "iterations")
+ if not self.args.pinocchio_only:
+ self.stopHandler(None, None)
"""
robotmanager:
@@ -110,6 +115,9 @@ class RobotManager:
# might be changed later if that seems more appropriate
def __init__(self, args):
self.args = args
+ self.pinocchio_only = args.pinocchio_only
+ self.simulation = args.simulation
+ self.gripper_flag = args.gripper
# load model
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
@@ -125,11 +133,13 @@ class RobotManager:
#viz.initViewer()
#viz.loadViewerModel()
#viz.display(q0)
- if args.gripper:
+ if self.gripper_flag and not self.pinocchio_only:
self.gripper = RobotiqGripper()
# initialize and connect the interfaces
self.simulation = args.simulation
- if not args.simulation:
+ if self.pinocchio_only:
+ self.q = pin.neutral(self.model)
+ elif not args.simulation:
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
self.rtde_io = RTDEIOInterface("192.168.1.102")
@@ -140,6 +150,8 @@ class RobotManager:
else:
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
self.n_joints = 6
@@ -155,7 +167,8 @@ class RobotManager:
# we're not saying it's qdd because it isn't directly (apparently)
# TODO: check that
self.acceleration = args.acceleration
- self.rtde_io.setSpeedSlider(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
# maybe you want to have them high and cap the rest with speed slider?
@@ -165,6 +178,9 @@ class RobotManager:
self.max_qd = 0.5
# maybe you want to scale the control signal
self.controller_speed_scaling = 1.0
+ # error limit
+ # TODO this is clik specific, put it in a better place
+ self.goal_error = args.goal_error
"""
getQ
@@ -175,22 +191,25 @@ class RobotManager:
also, the gripper is controlled separately so we'd need to do this somehow anyway
"""
def getQ(self):
- q = robot.rtde_receive.getActualQ()
- if self.args.gripper:
- self.gripper_pos = gripper.get_current_position()
- # the /255 is to get it dimensionless
- # the gap is 5cm
- # thus half the gap is 0.025m and we only do si units here
- q.append((self.gripper_pos / 255) * 0.025)
- q.append((self.gripper_pos / 255) * 0.025)
- else:
- # just fill it with zeros otherwise
- q.append(0.0)
- q.append(0.0)
+ if not self.pinocchio_only:
+ q = self.rtde_receive.getActualQ()
+ if self.args.gripper:
+ self.gripper_pos = gripper.get_current_position()
+ # the /255 is to get it dimensionless
+ # the gap is 5cm
+ # thus half the gap is 0.025m and we only do si units here
+ q.append((self.gripper_pos / 255) * 0.025)
+ q.append((self.gripper_pos / 255) * 0.025)
+ else:
+ # just fill it with zeros otherwise
+ q.append(0.0)
+ q.append(0.0)
# let's just have both options for getting q, it's just a 8d float list
# readability is a somewhat subjective quality after all
- q = np.array(q)
- self.q = q
+ q = np.array(q)
+ self.q = q
+ else:
+ return self.q
return q
"""
@@ -210,12 +229,12 @@ class RobotManager:
# np.clip is ok with bounds being scalar, it does what it should
# (but you can also give it an array)
qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
- if not self.simulation:
+ if not self.pinocchio_only:
# speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
- rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
else:
# this one takes all 8 elements of qd since we're still in pinocchio
- self.q = pin.integrate(model, self.q, qd * self.dt)
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
"""
@@ -238,11 +257,12 @@ class RobotManager:
Mtool = self.data.oMi[self.JOINT_ID]
if not self.args.visualize:
print("You can only specify the translation right now.")
- print("Here's where the robot is currently. Ensure you know what the base frame is first.")
-
- print("base frame end-effector pose from pinocchio:\n", \
- *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
- print("UR5e TCP:", *np.array(rtde_receive.getActualTCPPose()).round(4))
+ if not self.pinocchio_only:
+ print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
+ print("Here's where the robot is currently. Ensure you know what the base frame is first.")
+ print("base frame end-effector pose from pinocchio:\n", \
+ *self.data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4))
+ print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
# remain with the current orientation
# TODO: add something, probably rpy for orientation because it's the least number
# of numbers you need to type in
@@ -251,7 +271,7 @@ class RobotManager:
#Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
# do a while loop until this is parsed correctly
while True:
- goal = input("Please enter the target end effector position in the x.x,y.y,z.z format: ")
+ goal = input("Please enter the target end-effector position in the x.x,y.y,z.z format: ")
try:
e = "ok"
goal_list = goal.split(',')
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 34b958ff5c15cb061a2e633f9dfa78f898210782..62537f7122cb2bdb8a733bab151cf89bc95b5506 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -2,15 +2,19 @@ import pinocchio as pin
import numpy as np
import sys
import os
+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"
+ #urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
+ urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande.urdf')
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- mesh_dir = "../robot_descriptions/"
+ #mesh_dir = "../robot_descriptions/"
+ mesh_dir = files('ur_simple_control.robot_descriptions')
+ #mesh_dir_absolute = os.path.abspath(mesh_dir)
mesh_dir_absolute = os.path.abspath(mesh_dir)
shoulder_trans = np.array([0, 0, 0.1625134425523304])