diff --git a/demo/demo_quadruped.py b/demo/demo_quadruped.py index efd82d4d..9196469f 100644 --- a/demo/demo_quadruped.py +++ b/demo/demo_quadruped.py @@ -2,18 +2,18 @@ import subprocess import sys import time +from pathlib import Path import gepetto.corbaserver import matplotlib.pyplot as plt import numpy as np import pinocchio as pin import plot_utils as plut +import tsid from numpy import nan from numpy.linalg import norm as norm -import tsid - -sys.path += [os.getcwd() + "/../exercizes"] +sys.path += [Path.cwd().parent / "exercizes"] np.set_printoptions(precision=3, linewidth=200, suppress=True) @@ -43,7 +43,7 @@ DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps N_SIMULATION = 6000 # number of time steps simulated -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../models" urdf = path + "/quadruped/urdf/quadruped.urdf" vector = pin.StdVec_StdString() diff --git a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py index f3f1fc51..81b000e0 100644 --- a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py +++ b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py @@ -3,14 +3,13 @@ (c) MIPT """ -import os import time +from pathlib import Path import numpy as np import pinocchio as pin -from numpy.linalg import norm as norm - import tsid +from numpy.linalg import norm as norm np.set_printoptions(precision=3, linewidth=200, suppress=True) @@ -38,7 +37,7 @@ # Talos gripepr model (c) 2016, PAL Robotics, S.L. # Please use https://github.com/egordv/tsid_demo_closed_kinematic_chain repo for the model files -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "../../tsid_demo_closed_kinematic_chain/models/talos_gripper" urdf = path + "../../tsid_demo_closed_kinematic_chain/urdf/talos_gripper_half.urdf" vector = pin.StdVec_StdString() diff --git a/exercizes/ex_4_conf.py b/exercizes/ex_4_conf.py index cf0ab212..8eaa0a2b 100644 --- a/exercizes/ex_4_conf.py +++ b/exercizes/ex_4_conf.py @@ -3,7 +3,7 @@ @author: student """ -import os +from pathlib import Path import numpy as np import pinocchio as pin @@ -16,7 +16,7 @@ # robot parameters # ---------------------------------------------- -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../models/romeo" urdf = path + "/urdf/romeo.urdf" srdf = path + "/srdf/romeo_collision.srdf" diff --git a/exercizes/ex_4_long_conf.py b/exercizes/ex_4_long_conf.py index bec24a6d..603d1ff2 100644 --- a/exercizes/ex_4_long_conf.py +++ b/exercizes/ex_4_long_conf.py @@ -3,7 +3,7 @@ @author: student """ -import os +from pathlib import Path import numpy as np @@ -15,7 +15,7 @@ # robot parameters # ---------------------------------------------- -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../models/romeo" urdf = path + "/urdf/romeo.urdf" srdf = path + "/srdf/romeo_collision.srdf" diff --git a/exercizes/ex_4_talos_conf.py b/exercizes/ex_4_talos_conf.py index 8affe015..f2c0662e 100644 --- a/exercizes/ex_4_talos_conf.py +++ b/exercizes/ex_4_talos_conf.py @@ -3,7 +3,7 @@ @author: student """ -import os +from pathlib import Path import numpy as np import pinocchio as pin @@ -17,12 +17,12 @@ # robot parameters # ---------------------------------------------- -filename = str(os.path.dirname(os.path.abspath(__file__))) -urdf = "/talos_data/robots/talos_reduced.urdf" -modelPath = getModelPath(urdf) -urdf = modelPath + urdf -srdf = modelPath + "/talos_data/srdf/talos.srdf" -path = os.path.join(modelPath, "../..") +filename = str(Path(__file__).resolve().parent) +urdf = "talos_data/robots/talos_reduced.urdf" +modelPath = Path(getModelPath(urdf)) +urdf = modelPath / urdf +srdf = modelPath / "talos_data/srdf/talos.srdf" +path = modelPath / "../.." nv = 38 foot_scaling = 1.0 diff --git a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb index 440724ca..24e4d74c 100644 --- a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb +++ b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb @@ -16,6 +16,7 @@ "outputs": [], "source": [ "import sys\n", + "from pathlib import Path\n", "\n", "sys.path.append(\"..\")\n", "\n", @@ -28,7 +29,6 @@ "import tsid\n", "\n", "# import gepetto.corbaserver\n", - "import os\n", "\n", "import talos_arm_conf as conf" ] @@ -161,7 +161,7 @@ "outputs": [], "source": [ "robot_display = pin.RobotWrapper.BuildFromURDF(\n", - " conf.urdf, [os.path.join(conf.path, \"../..\")]\n", + " conf.urdf, [str(Path(conf.path) / \"../..\")]\n", ")\n", "# Viewer = pin.visualize.GepettoVisualizer\n", "Viewer = pin.visualize.MeshcatVisualizer\n", diff --git a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb index 72dd187e..f8439bd9 100644 --- a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb +++ b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb @@ -236,7 +236,7 @@ "import matplotlib.pyplot as plt\n", "import numpy as np\n", "from numpy.linalg import norm\n", - "import os\n", + "from pathlib import Path\n", "import time as tmp\n", "\n", "# import the library TSID for the Whole-Body Controller\n", @@ -353,7 +353,7 @@ "source": [ "# Creation of the robot wrapper for gepetto viewer (graphical interface)\n", "robot_display = pin.RobotWrapper.BuildFromURDF(\n", - " urdf, [os.path.join(path, \"../..\")], pin.JointModelFreeFlyer()\n", + " urdf, [str(Path(path) / \"../..\")], pin.JointModelFreeFlyer()\n", ")\n", "# Viewer = pin.visualize.GepettoVisualizer\n", "Viewer = pin.visualize.MeshcatVisualizer\n", diff --git a/exercizes/romeo_conf.py b/exercizes/romeo_conf.py index 9ac83ebb..20cb8c01 100644 --- a/exercizes/romeo_conf.py +++ b/exercizes/romeo_conf.py @@ -3,7 +3,7 @@ @author: student """ -import os +from pathlib import Path import numpy as np import pinocchio as pin @@ -72,7 +72,7 @@ LF_SPHERE_COLOR = (0, 0, 1, 0.5) LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5) -filename = str(os.path.dirname(os.path.abspath(__file__))) -path = filename + "/../models/romeo" -urdf = path + "/urdf/romeo.urdf" -srdf = path + "/srdf/romeo_collision.srdf" +filename = Path(__file__).resolve().parent +path = filename / "../models/romeo" +urdf = path / "urdf/romeo.urdf" +srdf = path / "srdf/romeo_collision.srdf" diff --git a/exercizes/talos_arm_conf.py b/exercizes/talos_arm_conf.py index a8a198ef..dd8f00d4 100644 --- a/exercizes/talos_arm_conf.py +++ b/exercizes/talos_arm_conf.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np from example_robot_data.robots_loader import getModelPath @@ -45,5 +47,5 @@ EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5) urdf = "/talos_data/robots/talos_left_arm.urdf" -path = getModelPath(urdf) -urdf = path + urdf +path = Path(getModelPath(urdf)) +urdf = path / urdf diff --git a/exercizes/talos_conf.py b/exercizes/talos_conf.py index 4e8d9333..40098f98 100644 --- a/exercizes/talos_conf.py +++ b/exercizes/talos_conf.py @@ -1,4 +1,4 @@ -import os +from pathlib import Path import numpy as np import pinocchio as pin @@ -104,7 +104,7 @@ LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5) urdf = "/talos_data/robots/talos_reduced.urdf" -path = getModelPath(urdf) -urdf = path + urdf -srdf = path + "/talos_data/srdf/talos.srdf" -path = os.path.join(path, "../..") +path = Path(getModelPath(urdf)) +urdf = path / urdf +srdf = path / "talos_data/srdf/talos.srdf" +path = path / "../.." diff --git a/exercizes/ur5_conf.py b/exercizes/ur5_conf.py index cdc1a612..3ca6c139 100644 --- a/exercizes/ur5_conf.py +++ b/exercizes/ur5_conf.py @@ -3,7 +3,7 @@ @author: student """ -from os.path import join +from pathlib import Path import numpy as np from example_robot_data.robots_loader import getModelPath @@ -54,6 +54,6 @@ urdf = "ur_description/urdf/ur5_robot.urdf" -path = getModelPath(urdf) -urdf = join(path, urdf) -path = join(path, "../..") +path = Path(getModelPath(urdf)) +urdf = path / urdf +path = path / "../.." diff --git a/exercizes/ur5_reaching_conf.py b/exercizes/ur5_reaching_conf.py index e8bb7313..f2501167 100644 --- a/exercizes/ur5_reaching_conf.py +++ b/exercizes/ur5_reaching_conf.py @@ -3,7 +3,7 @@ @author: student """ -from os.path import join +from pathlib import Path import numpy as np from example_robot_data.robots_loader import getModelPath @@ -54,6 +54,6 @@ urdf = "ur_description/urdf/ur5_robot.urdf" -path = getModelPath(urdf) -urdf = join(path, urdf) -path = join(path, "../..") +path = Path(getModelPath(urdf)) +urdf = path / urdf +path = path / "../.." diff --git a/tests/python/test_Contact.py b/tests/python/test_Contact.py index 464eb1af..3e5eee5e 100644 --- a/tests/python/test_Contact.py +++ b/tests/python/test_Contact.py @@ -1,4 +1,4 @@ -import os +from pathlib import Path import numpy as np import pinocchio as se3 @@ -11,7 +11,7 @@ tol = 1e-5 -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = se3.StdVec_StdString() diff --git a/tests/python/test_ContactPoint.py b/tests/python/test_ContactPoint.py index 1345389d..4f4aad3f 100644 --- a/tests/python/test_ContactPoint.py +++ b/tests/python/test_ContactPoint.py @@ -1,8 +1,7 @@ -import os +from pathlib import Path import numpy as np import pinocchio as se3 - import tsid print("") @@ -10,7 +9,8 @@ print("") tol = 1e-5 -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) + path = filename + "/../../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = se3.StdVec_StdString() diff --git a/tests/python/test_Formulation.py b/tests/python/test_Formulation.py index 652901fd..3fa9fc88 100644 --- a/tests/python/test_Formulation.py +++ b/tests/python/test_Formulation.py @@ -1,16 +1,15 @@ -import os +from pathlib import Path import numpy as np import pinocchio as se3 -from numpy.linalg import norm - import tsid +from numpy.linalg import norm print("") print("Test InvDyn") print("") -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = se3.StdVec_StdString() diff --git a/tests/python/test_Gravity.py b/tests/python/test_Gravity.py index de8c9e9a..1b0e4f8b 100644 --- a/tests/python/test_Gravity.py +++ b/tests/python/test_Gravity.py @@ -1,7 +1,6 @@ -import os +from pathlib import Path import pinocchio as se3 - import tsid print("") @@ -9,7 +8,7 @@ print("") -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = se3.StdVec_StdString() diff --git a/tests/python/test_RobotWrapper.py b/tests/python/test_RobotWrapper.py index 7fa1df85..9bf240c5 100644 --- a/tests/python/test_RobotWrapper.py +++ b/tests/python/test_RobotWrapper.py @@ -1,4 +1,4 @@ -import os +from pathlib import Path import numpy as np import pinocchio as se3 @@ -10,7 +10,7 @@ print("") -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = se3.StdVec_StdString() diff --git a/tests/python/test_Tasks.py b/tests/python/test_Tasks.py index 201bf104..d4122cf5 100644 --- a/tests/python/test_Tasks.py +++ b/tests/python/test_Tasks.py @@ -1,4 +1,4 @@ -import os +from pathlib import Path import numpy as np import pinocchio as pin @@ -14,7 +14,7 @@ tol = 1e-5 -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = pin.StdVec_StdString() @@ -204,7 +204,7 @@ print("") tol = 1e-5 -filename = str(os.path.dirname(os.path.abspath(__file__))) +filename = str(Path(__file__).resolve().parent) path = filename + "/../../models/romeo" urdf = path + "/urdf/romeo.urdf" vector = pin.StdVec_StdString()