diff --git a/examples/cart_pulling/path6d_from_path2d.py b/examples/cart_pulling/path6d_from_path2d.py
new file mode 100644
index 0000000000000000000000000000000000000000..00c0d23cbd060d4f2dacbe529b3bf2ba77618e0b
--- /dev/null
+++ b/examples/cart_pulling/path6d_from_path2d.py
@@ -0,0 +1,105 @@
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.cartesian_space import getClikArgs
+from smc.path_generation.planner import getPlanningArgs
+from smc.control.optimal_control.croco_mpc_path_following import initializePastData
+from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3_fixed
+from smc.path_generation.path_math.path_to_trajectory import path2D_timed
+from smc.control.optimal_control.util import get_OCP_args
+
+from smc.path_generation.path_math.cart_pulling_path_math import (
+ time_base_path,
+ construct_EE_path,
+)
+
+import yaml
+import numpy as np
+from functools import partial
+import numpy as np
+import time
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ parser = getPlanningArgs(parser)
+ parser.add_argument(
+ "--handlebar-height",
+ type=float,
+ default=0.5,
+ help="heigh of handlebar of the cart to be pulled",
+ )
+ parser.add_argument(
+ "--base-to-handlebar-preferred-distance",
+ type=float,
+ default=0.5,
+ help="prefered path arclength from mobile base position to handlebar",
+ )
+ args = parser.parse_args()
+ return args
+
+
+args = get_args()
+args.past_window_size = 100
+args.robot = "heron"
+robot = getRobotFromArgs(args)
+robot._step()
+robot.visualizer_manager.sendCommand({"q": robot.q})
+input("press Enter to continue")
+print(robot.q)
+
+max_base_v = np.linalg.norm(robot._max_v[:2])
+dt = 1 / args.ctrl_freq
+
+path2D_handlebar = initializePastData(
+ args, robot.T_w_e, robot.base_position2D, max_base_v
+)
+# path2D_handlebar = np.linspace(0, 1, args.past_window_size).reshape((-1, 1))
+# path2D_handlebar = np.hstack((path2D_handlebar, path2D_handlebar))
+print(path2D_handlebar.shape)
+path3D_handlebar = np.hstack(
+ (path2D_handlebar, np.ones((len(path2D_handlebar), 1)) * robot.T_w_e.translation[2])
+)
+# print(path3D_handlebar.shape)
+# robot.visualizer_manager.sendCommand({"path": path2D_handlebar})
+robot.visualizer_manager.sendCommand({"path": path3D_handlebar})
+print(
+ "we initialize path ee poses with this 2D path (z axis adjusted just for visual clarity"
+)
+print(path3D_handlebar)
+# print(path2D_handlebar)
+input("press Enter to continue")
+
+# path you already traversed
+# time_past = np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+x = np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+# x = np.linspace(0.0, 2.0, 200)
+x = x.reshape((-1, 1))
+y = np.sin(x)
+past_data = {}
+past_data["path2D_untimed"] = np.hstack((x, y))
+
+# path you get from path planner
+x = np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+# x = np.linspace(2.0, 4.0, 200)
+x = x.reshape((-1, 1))
+y = np.sin(x)
+path2D_untimed_base = np.hstack((x, y))
+
+p = path2D_untimed_base[-1]
+path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1, 2))
+
+path_base = time_base_path(args, path2D_untimed_base, max_base_v)
+print(path_base)
+pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
+
+for i in range(100):
+ visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
+ # visualizer_manager.sendCommand({"frame_path": some_path})
+ visualizer_manager.sendCommand({"path": path_base})
+ time.sleep(1)
+print("send em")
+
+time.sleep(10)
+visualizer_manager.terminateProcess()
diff --git a/examples/graz/cartesian_pose_command_server.py b/examples/graz/cartesian_pose_command_server.py
index e63702faf1631862f232ca951325dc173a4d3957..8349916faccc471345173177826850aee4d174ad 100644
--- a/examples/graz/cartesian_pose_command_server.py
+++ b/examples/graz/cartesian_pose_command_server.py
@@ -1,11 +1,10 @@
+from smc import getMinimalArgParser
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.multiprocessing.networking.server import server_sender
+from smc.multiprocessing.networking.client import client_receiver
+
import pinocchio as pin
import numpy as np
-from smc.managers import (
- ProcessManager,
- getMinimalArgParser,
-)
-from smc.networking.server import server_sender
-from smc.networking.client import client_receiver
import time
diff --git a/examples/graz/clik_client.py b/examples/graz/clik_client.py
index 019f99e51e542fa25df83a3ecbf9a2a5cbaa3f7a..aca40dc8f90f809159bfb3804b72cce1f7e3edeb 100644
--- a/examples/graz/clik_client.py
+++ b/examples/graz/clik_client.py
@@ -1,15 +1,20 @@
+from smc.multiprocessing.networking.client import client_receiver, client_sender
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.robots.interfaces.force_torque_sensor_interface import (
+ ForceTorqueOnSingleArmWrist,
+)
+from smc.control.cartesian_space.ik_solvers import getIKSolver
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveL
+from smc.control.cartesian_space.utils import getClikArgs
+from smc.control.control_loop_manager import ControlLoopManager
+from smc import (
+ getMinimalArgParser,
+ getRobotFromArgs,
+)
+
import pinocchio as pin
import numpy as np
from functools import partial
-from smc.networking.client import client_receiver, client_sender
-from smc.networking.server import server_sender
-from smc.clik.clik import *
-from smc.managers import (
- ProcessManager,
- getMinimalArgParser,
- ControlLoopManager,
- RobotManager,
-)
def get_args():
@@ -30,10 +35,10 @@ def get_args():
def controlLoopClikExternalGoal(
- robot: RobotManager,
+ robot: ForceTorqueOnSingleArmWrist,
receiver: ProcessManager,
sender: ProcessManager,
- clik_controller,
+ ik_solver,
i,
past_data,
):
@@ -46,8 +51,8 @@ def controlLoopClikExternalGoal(
log_item = {}
save_past_dict = {}
- q = robot.getQ()
- T_w_e = robot.getT_w_e()
+ q = robot.q
+ T_w_e = robot.T_w_e
data = receiver.getData()
T_goal = data["T_goal"]
# print(T_goal)
@@ -64,29 +69,26 @@ def controlLoopClikExternalGoal(
)
if qd_cmd is None:
print("the controller you chose didn't work, using dampedPseudoinverse instead")
- qd_cmd = dampedPseudoinverse(1e-2, J, err_vector)
+ qd_cmd = ik_solver(1e-2, J, err_vector)
# NOTE: this will also receive a velocity
# i have no idea what to do with that velocity
- robot.sendQd(qd_cmd)
+ robot.sendVelocityCommand(qd_cmd)
# NOTE: this one is in the base frame! (as it should be)
# it is saved in robotmanager (because it's a private variable,
# yes this is evil bla bla)
- wrench_in_robot = robot.getWrench()
- robot._getWrench()
- wrench = robot.getWrench()
- robot.wrench = wrench_in_robot
- sender.sendCommand({"wrench": wrench})
+ # NOTE: idk whether i'm sending this or wrench_ee lule
+ sender.sendCommand({"wrench": robot.wrench_base})
- log_item["qs"] = robot.getQ().reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
log_item["err_vector"] = err_vector
return breakFlag, save_past_dict, log_item
if __name__ == "__main__":
args = get_args()
- robot = RobotManager(args)
+ robot = getRobotFromArgs(args)
translation = np.array(args.init_pose[:3])
orientation = pin.Quaternion(np.array(args.init_pose[3:]))
init_goal = pin.SE3(orientation, translation)
@@ -102,7 +104,7 @@ if __name__ == "__main__":
client_receiver,
{"T_goal": pin.SE3.Identity(), "v": np.zeros(6)},
4,
- init_value={"T_goal": robot.getT_w_e(), "v": np.zeros(6)},
+ init_value={"T_goal": robot.T_w_e, "v": np.zeros(6)},
)
# wrench_sender: 6666
args.port = args.port_wrench
@@ -112,7 +114,7 @@ if __name__ == "__main__":
"dqs": np.zeros((robot.model.nv,)),
"err_vector": np.zeros((6,)),
}
- clik_controller = getClikController(args, robot)
+ clik_controller = getIKSolver(args, robot)
control_loop = partial(
controlLoopClikExternalGoal, robot, receiver, sender, clik_controller
)
@@ -120,15 +122,12 @@ if __name__ == "__main__":
loop_manager.run()
# get expected behaviour here (library can't know what the end is - you have to do this here)
- if not args.pinocchio_only:
+ if args.real:
robot.stopRobot()
if args.save_log:
- robot.log_manager.plotAllControlLoops()
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
if args.visualizer:
robot.killManipulatorVisualizer()
-
- if args.save_log:
- robot.log_manager.saveLog()
- # loop_manager.stopHandler(None, None)
diff --git a/python/smc.egg-info/SOURCES.txt b/python/smc.egg-info/SOURCES.txt
index 5772872e2ac62db9911730a91afc02a76bf290ed..7063d2e2f5226b842a6f07eb62e33bdd05bf2379 100644
--- a/python/smc.egg-info/SOURCES.txt
+++ b/python/smc.egg-info/SOURCES.txt
@@ -1,9 +1,356 @@
LICENSE.txt
README.md
setup.py
+initial_python_solution/main.py
+initial_python_solution/make_run.py
+initial_python_solution/manipulator_visual_motion_analyzer.py
+initial_python_solution/__pycache__/make_run.cpython-310.pyc
+initial_python_solution/__pycache__/make_run.cpython-311.pyc
+initial_python_solution/arms/another_debug_dh
+initial_python_solution/arms/j2n6s300_dh_params
+initial_python_solution/arms/j2s7s300_dh_params
+initial_python_solution/arms/kinova_jaco_params
+initial_python_solution/arms/kuka_lbw_iiwa_dh_params
+initial_python_solution/arms/lbr_iiwa_jos_jedan_dh
+initial_python_solution/arms/robot_parameters2
+initial_python_solution/arms/testing_dh_parameters
+initial_python_solution/arms/ur10e_dh_parameters_from_the_ur_site
+initial_python_solution/arms/ur5e_dh
+initial_python_solution/example_code/blit_in_tk.py
+initial_python_solution/example_code/blit_test.py
+initial_python_solution/example_code/dark_background_example.py
+initial_python_solution/example_code/ellipse.py
+initial_python_solution/example_code/embedding_in_tk_sgskip.py
+initial_python_solution/example_code/manager_anim.py
+initial_python_solution/example_code/run_classic_ik_algs.py
+initial_python_solution/robot_stuff/InverseKinematics.py
+initial_python_solution/robot_stuff/drawing.py
+initial_python_solution/robot_stuff/drawing_for_anim.py
+initial_python_solution/robot_stuff/follow_curve.py
+initial_python_solution/robot_stuff/forw_kinm.py
+initial_python_solution/robot_stuff/inv_kinm.py
+initial_python_solution/robot_stuff/joint_as_hom_mat.py
+initial_python_solution/robot_stuff/utils.py
+initial_python_solution/robot_stuff/webots_api_helper_funs.py
+initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/follow_curve.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/follow_curve.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/utils.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/utils.cpython-311.pyc
+initial_python_solution/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc
+initial_python_solution/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc
smc/__init__.py
smc.egg-info/PKG-INFO
smc.egg-info/SOURCES.txt
smc.egg-info/dependency_links.txt
smc.egg-info/not-zip-safe
-smc.egg-info/top_level.txt
\ No newline at end of file
+smc.egg-info/top_level.txt
+smc/control/__init__.py
+smc/control/control_loop_manager.py
+smc/control/freedrive.py
+smc/control/cartesian_space/__init__.py
+smc/control/cartesian_space/cartesian_space_compliant_control.py
+smc/control/cartesian_space/cartesian_space_point_to_point.py
+smc/control/cartesian_space/cartesian_space_trajectory_following.py
+smc/control/cartesian_space/ik_solvers.py
+smc/control/cartesian_space/utils.py
+smc/control/cartesian_space/experimental/traiettoria.py
+smc/control/cartesian_space/experimental/whole_body_clik.py
+smc/control/dmp/__init__.py
+smc/control/dmp/dmp.py
+smc/control/dmp/notes.md
+smc/control/joint_space/__init__.py
+smc/control/joint_space/joint_space_point_to_point.py
+smc/control/joint_space/joint_space_trajectory_following.py
+smc/control/optimal_control/__init__.py
+smc/control/optimal_control/abstract_croco_ocp.py
+smc/control/optimal_control/create_pinocchio_casadi_ocp.py
+smc/control/optimal_control/croco_mpc_path_following.py
+smc/control/optimal_control/croco_mpc_point_to_point.py
+smc/control/optimal_control/notes.md
+smc/control/optimal_control/path_following_croco_ocp.py
+smc/control/optimal_control/point_to_point_croco_ocp.py
+smc/control/optimal_control/util.py
+smc/logging/__init__.py
+smc/logging/logger.py
+smc/multiprocessing/__init__.py
+smc/multiprocessing/multiprocess_manager.py
+smc/multiprocessing/process_manager.py
+smc/multiprocessing/networking/__init__.py
+smc/multiprocessing/networking/client.py
+smc/multiprocessing/networking/message_specs.proto
+smc/multiprocessing/networking/message_specs_pb2.py
+smc/multiprocessing/networking/notes.md
+smc/multiprocessing/networking/server.py
+smc/multiprocessing/networking/todos.md
+smc/multiprocessing/networking/util.py
+smc/path_generation/OLD_sebas_path_generator_WITH_EVERYTHING.py
+smc/path_generation/README.md
+smc/path_generation/__init__.py
+smc/path_generation/cartesian_to_joint_space_path_mapper.py
+smc/path_generation/path_generator.py
+smc/path_generation/planner.py
+smc/path_generation/robot_params.yaml
+smc/path_generation/scene_updater.py
+smc/path_generation/setup.py
+smc/path_generation/tunnel_mpc_params.yaml
+smc/path_generation/maps/premade_maps.py
+smc/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO
+smc/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt
+smc/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt
+smc/path_generation/mobile_manipulation_challenge.egg-info/requires.txt
+smc/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt
+smc/path_generation/path_math/cart_pulling_path_math.py
+smc/path_generation/path_math/path2d_to_6d.py
+smc/path_generation/path_math/path_to_trajectory.py
+smc/path_generation/star_navigation/README.md
+smc/path_generation/star_navigation/setup.py
+smc/path_generation/star_navigation/config/__init__.py
+smc/path_generation/star_navigation/config/load_config.py
+smc/path_generation/star_navigation/config/scene.py
+smc/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml
+smc/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml
+smc/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml
+smc/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml
+smc/path_generation/star_navigation/config/params/robot_params.yaml
+smc/path_generation/star_navigation/config/params/soads_ctrl_params.yaml
+smc/path_generation/star_navigation/motion_control/__init__.py
+smc/path_generation/star_navigation/motion_control/dmp/__init__.py
+smc/path_generation/star_navigation/motion_control/dmp/dmp.py
+smc/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py
+smc/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py
+smc/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py
+smc/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore
+smc/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py
+smc/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py
+smc/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py
+smc/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py
+smc/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py
+smc/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore
+smc/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py
+smc/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py
+smc/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py
+smc/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore
+smc/path_generation/star_navigation/motion_control/soads/__init__.py
+smc/path_generation/star_navigation/motion_control/soads/soads.py
+smc/path_generation/star_navigation/robot/__init__.py
+smc/path_generation/star_navigation/robot/bicycle.py
+smc/path_generation/star_navigation/robot/mobile_robot.py
+smc/path_generation/star_navigation/robot/omnidirectional.py
+smc/path_generation/star_navigation/robot/unicycle.py
+smc/path_generation/star_navigation/scripts/compute_time.py
+smc/path_generation/star_navigation/scripts/run_simulation.py
+smc/path_generation/star_navigation/scripts/simulate_several_controllers.py
+smc/path_generation/star_navigation/scripts/simulate_several_initpos.py
+smc/path_generation/star_navigation/scripts/test_soads.py
+smc/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
+smc/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
+smc/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt
+smc/path_generation/star_navigation/star_navigation.egg-info/top_level.txt
+smc/path_generation/star_navigation/starworlds/LICENSE
+smc/path_generation/star_navigation/starworlds/README.md
+smc/path_generation/star_navigation/starworlds/requirements.txt
+smc/path_generation/star_navigation/starworlds/setup.py
+smc/path_generation/star_navigation/starworlds/obstacles/__init__.py
+smc/path_generation/star_navigation/starworlds/obstacles/ellipse.py
+smc/path_generation/star_navigation/starworlds/obstacles/motion_model.py
+smc/path_generation/star_navigation/starworlds/obstacles/obstacle.py
+smc/path_generation/star_navigation/starworlds/obstacles/polygon.py
+smc/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py
+smc/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py
+smc/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py
+smc/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py
+smc/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py
+smc/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py
+smc/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO
+smc/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt
+smc/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt
+smc/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt
+smc/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt
+smc/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py
+smc/path_generation/star_navigation/starworlds/tests/test_obstacles.py
+smc/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py
+smc/path_generation/star_navigation/starworlds/tests/test_timing.py
+smc/path_generation/star_navigation/starworlds/tests/test_utils.py
+smc/path_generation/star_navigation/starworlds/utils/__init__.py
+smc/path_generation/star_navigation/starworlds/utils/cg.py
+smc/path_generation/star_navigation/starworlds/utils/misc.py
+smc/path_generation/star_navigation/visualization/__init__.py
+smc/path_generation/star_navigation/visualization/scene_gui.py
+smc/path_generation/star_navigation/visualization/video_writer.py
+smc/path_generation/star_navigation/visualization/figures/.gitignore
+smc/path_generation/star_navigation/visualization/videos/.gitignore
+smc/path_generation/starworld_tunnel_mpc/README.md
+smc/path_generation/starworld_tunnel_mpc/setup.py
+smc/path_generation/starworld_tunnel_mpc/config/__init__.py
+smc/path_generation/starworld_tunnel_mpc/config/load_config.py
+smc/path_generation/starworld_tunnel_mpc/config/scene.py
+smc/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml
+smc/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml
+smc/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml
+smc/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml
+smc/path_generation/starworld_tunnel_mpc/motion_control/__init__.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py
+smc/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore
+smc/path_generation/starworld_tunnel_mpc/robot/__init__.py
+smc/path_generation/starworld_tunnel_mpc/robot/bicycle.py
+smc/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py
+smc/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py
+smc/path_generation/starworld_tunnel_mpc/robot/unicycle.py
+smc/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
+smc/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
+smc/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/dependency_links.txt
+smc/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
+smc/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/top_level.txt
+smc/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py
+smc/path_generation/starworld_tunnel_mpc/tests/test_soads.py
+smc/path_generation/starworld_tunnel_mpc/visualization/__init__.py
+smc/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py
+smc/path_generation/starworlds/LICENSE
+smc/path_generation/starworlds/README.md
+smc/path_generation/starworlds/requirements.txt
+smc/path_generation/starworlds/setup.py
+smc/path_generation/starworlds/obstacles/__init__.py
+smc/path_generation/starworlds/obstacles/ellipse.py
+smc/path_generation/starworlds/obstacles/motion_model.py
+smc/path_generation/starworlds/obstacles/obstacle.py
+smc/path_generation/starworlds/obstacles/polygon.py
+smc/path_generation/starworlds/obstacles/starshaped_obstacle.py
+smc/path_generation/starworlds/obstacles/starshaped_polygon.py
+smc/path_generation/starworlds/obstacles/starshaped_primitive_combination.py
+smc/path_generation/starworlds/starshaped_hull/__init__.py
+smc/path_generation/starworlds/starshaped_hull/cluster_and_starify.py
+smc/path_generation/starworlds/starshaped_hull/starshaped_hull.py
+smc/path_generation/starworlds/starworlds.egg-info/PKG-INFO
+smc/path_generation/starworlds/starworlds.egg-info/SOURCES.txt
+smc/path_generation/starworlds/starworlds.egg-info/dependency_links.txt
+smc/path_generation/starworlds/starworlds.egg-info/requires.txt
+smc/path_generation/starworlds/starworlds.egg-info/top_level.txt
+smc/path_generation/starworlds/tests/test_cluster_and_starify.py
+smc/path_generation/starworlds/tests/test_obstacles.py
+smc/path_generation/starworlds/tests/test_starshaped_hull.py
+smc/path_generation/starworlds/tests/test_timing.py
+smc/path_generation/starworlds/tests/test_utils.py
+smc/path_generation/starworlds/utils/__init__.py
+smc/path_generation/starworlds/utils/cg.py
+smc/path_generation/starworlds/utils/misc.py
+smc/robots/__init__.py
+smc/robots/abstract_simulated_robot.py
+smc/robots/notes_on_getting_model_from_urdf.md
+smc/robots/robotmanager_abstract.py
+smc/robots/utils.py
+smc/robots/grippers/__init__.py
+smc/robots/grippers/abstract_gripper.py
+smc/robots/grippers/on_robot/twofg.py
+smc/robots/grippers/robotiq/robotiq_gripper.py
+smc/robots/implementations/__init__.py
+smc/robots/implementations/heron.py
+smc/robots/implementations/mir.py
+smc/robots/implementations/mobile_yumi.py
+smc/robots/implementations/ur5e.py
+smc/robots/interfaces/dual_arm_interface.py
+smc/robots/interfaces/force_torque_sensor_interface.py
+smc/robots/interfaces/mobile_base_interface.py
+smc/robots/interfaces/single_arm_interface.py
+smc/robots/robot_descriptions/TODO_PUT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF
+smc/robots/robot_descriptions/__init__.py
+smc/robots/robot_descriptions/my_robot_calibration.yaml
+smc/robots/robot_descriptions/yumi.urdf
+smc/robots/robot_descriptions/yumi_local.urdf
+smc/robots/robot_descriptions/config/__init__.py
+smc/robots/robot_descriptions/config/ur5e/__init__.py
+smc/robots/robot_descriptions/config/ur5e/default_kinematics.yaml
+smc/robots/robot_descriptions/config/ur5e/joint_limits.yaml
+smc/robots/robot_descriptions/config/ur5e/physical_parameters.yaml
+smc/robots/robot_descriptions/config/ur5e/visual_parameters.yaml
+smc/robots/robot_descriptions/meshes/__init__.py
+smc/robots/robot_descriptions/meshes/robotiq_85_coupler.stl
+smc/robots/robot_descriptions/meshes/ur5e/__init__.py
+smc/robots/robot_descriptions/meshes/ur5e/robotiq_85_coupler.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/__init__.py
+smc/robots/robot_descriptions/meshes/ur5e/collision/base.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/finger_1-collision.dae
+smc/robots/robot_descriptions/meshes/ur5e/collision/finger_2-collision.dae
+smc/robots/robot_descriptions/meshes/ur5e/collision/forearm.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/hand-e-collision.dae
+smc/robots/robot_descriptions/meshes/ur5e/collision/shoulder.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/upperarm.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/wrist1.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/wrist2.stl
+smc/robots/robot_descriptions/meshes/ur5e/collision/wrist3.stl
+smc/robots/robot_descriptions/meshes/ur5e/visual/__init__.py
+smc/robots/robot_descriptions/meshes/ur5e/visual/base.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/finger_1.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/finger_2.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/forearm.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/hand-e.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/shoulder.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/upperarm.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/wrist1.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/wrist2.dae
+smc/robots/robot_descriptions/meshes/ur5e/visual/wrist3.dae
+smc/robots/robot_descriptions/urdf/__init__.py
+smc/robots/robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf
+smc/robots/robot_descriptions/urdf/ur5e_with_robotiq_hande_FIXED_PATHS.urdf
+smc/util/__init__.py
+smc/util/calib_board_hacks.py
+smc/util/define_random_goal.py
+smc/util/draw_path.py
+smc/util/encapsulating_ellipses.py
+smc/util/freedrive.py
+smc/util/map2DPathTo3DPlane.py
+smc/vision/__init__.py
+smc/vision/vision.py
+smc/visualization/__init__.py
+smc/visualization/manipulator_comparison_visualizer.py
+smc/visualization/manipulator_visual_motion_analyzer.py
+smc/visualization/path_visualizer.py
+smc/visualization/plotters.py
+smc/visualization/visualizer.py
+smc/visualization/arms/another_debug_dh
+smc/visualization/arms/j2n6s300_dh_params
+smc/visualization/arms/j2s7s300_dh_params
+smc/visualization/arms/kinova_jaco_params
+smc/visualization/arms/kuka_lbw_iiwa_dh_params
+smc/visualization/arms/lbr_iiwa_jos_jedan_dh
+smc/visualization/arms/robot_parameters2
+smc/visualization/arms/testing_dh_parameters
+smc/visualization/arms/ur10e_dh_parameters_from_the_ur_site
+smc/visualization/arms/ur5e_dh
+smc/visualization/meshcat_viewer_wrapper/__init__.py
+smc/visualization/meshcat_viewer_wrapper/colors.py
+smc/visualization/meshcat_viewer_wrapper/visualizer.py
+smc/visualization/obsolete_to_be_removed/main.py
+smc/visualization/obsolete_to_be_removed/make_run.py
+smc/visualization/robot_stuff/InverseKinematics.py
+smc/visualization/robot_stuff/drawing.py
+smc/visualization/robot_stuff/drawing_for_anim.py
+smc/visualization/robot_stuff/follow_curve.py
+smc/visualization/robot_stuff/forw_kinm.py
+smc/visualization/robot_stuff/inv_kinm.py
+smc/visualization/robot_stuff/joint_as_hom_mat.py
+smc/visualization/robot_stuff/utils.py
+smc/visualization/robot_stuff/webots_api_helper_funs.py
\ No newline at end of file
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
index 57299aafdb0b103f14df354e90d1bbc98ad12b9b..bd1ec8913f3221895e32b6924751640e5138a172 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -305,23 +305,24 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
return breakFlag, save_past_dict, log_item
-def initializePastData(args: Namespace, T_w_e: SE3, p_base: np.ndarray) -> np.ndarray:
+def initializePastData(args: Namespace, T_w_e: SE3, p_base: np.ndarray, max_base_v: float) -> np.ndarray:
# prepopulate past data to make base and cart be on the same path in the past
# (which didn't actually happen because this just started)
p_ee = T_w_e.translation[:2]
straight_line_path = np.linspace(p_ee, p_base, args.past_window_size)
+ straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
# time it the same way the base path is timed
- time_past = np.linspace(
- 0.0, args.past_window_size * (1 / args.ctrl_freq), args.past_window_size
- )
- s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)
- path2D_handlebar = np.hstack(
- (
- np.interp(s, time_past, straight_line_path[:, 0]).reshape((-1, 1)),
- np.interp(s, time_past, straight_line_path[:, 1]).reshape((-1, 1)),
- )
- )
- #return path2D_handlebar
+# time_past = np.linspace(
+# 0.0, args.past_window_size * (1 / args.ctrl_freq), args.past_window_size
+# )
+# s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)
+# path2D_handlebar = np.hstack(
+# (
+# np.interp(s, time_past, straight_line_path[:, 0]).reshape((-1, 1)),
+# np.interp(s, time_past, straight_line_path[:, 1]).reshape((-1, 1)),
+# )
+# )
+# return path2D_handlebar
return straight_line_path
@@ -344,7 +345,9 @@ def BaseAndEEPathFollowingMPC(
ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
ocp.solveInitialOCP(x0)
- path2D_handlebar = initializePastData(args, T_w_e, robot.q[:2])
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+
+ path2D_handlebar = initializePastData(args, T_w_e, robot.q[:2], float(max_base_v))
if type(path_planner) == types.FunctionType:
controlLoop = partial(
diff --git a/python/smc/multiprocessing/networking/client.py b/python/smc/multiprocessing/networking/client.py
index 1c25cf54e927487b85123365a8bc0b1db2531ab2..1d617d261e81013308a472da618554153eb9f620 100644
--- a/python/smc/multiprocessing/networking/client.py
+++ b/python/smc/multiprocessing/networking/client.py
@@ -1,13 +1,16 @@
-import socket
-from multiprocessing import shared_memory
+from typing import Any
+from smc.multiprocessing.networking.util import DictPb2EncoderDecoder
+
from google.protobuf.internal.encoder import _VarintBytes
from google.protobuf.internal.decoder import _DecodeVarint32
-from ur_simple_control.networking.util import DictPb2EncoderDecoder
+from multiprocessing import shared_memory, Queue
+import socket
import pickle
import time
+from argparse import Namespace
-def client_receiver(args, init_command, shm_name, lock):
+def client_receiver(args: Namespace, init_command, shm_name: str, lock):
"""
client
-------
@@ -113,7 +116,7 @@ def client_receiver(args, init_command, shm_name, lock):
print("NETWORKING_CLIENT: caught KeyboardInterrupt, i'm out")
-def client_sender(args, init_command, queue):
+def client_sender(args: Namespace, init_command: dict[str, Any], queue: Queue):
"""
server
-------
diff --git a/python/smc/multiprocessing/networking/server.py b/python/smc/multiprocessing/networking/server.py
index dac395797282cab1efdb93058197710075e1f2de..a81f093ef21414f1d87fd5d2479b3cd8b84a3e12 100644
--- a/python/smc/multiprocessing/networking/server.py
+++ b/python/smc/multiprocessing/networking/server.py
@@ -1,6 +1,7 @@
+from smc.multiprocessing.networking.util import DictPb2EncoderDecoder
+
import socket
from google.protobuf.internal.encoder import _VarintBytes
-from ur_simple_control.networking.util import DictPb2EncoderDecoder
def server_sender(args, init_command, queue):
diff --git a/python/smc/multiprocessing/networking/util.py b/python/smc/multiprocessing/networking/util.py
index 4b3bf8b00e23653740fa4e0aadf0e9918ebd55ca..828cabdd8c43e3a949f5991a1c393a657e2f9191 100644
--- a/python/smc/multiprocessing/networking/util.py
+++ b/python/smc/multiprocessing/networking/util.py
@@ -1,6 +1,7 @@
# TODO: make protobuf an optional import
# then if it's not there use pickling as default
-import ur_simple_control.networking.message_specs_pb2 as message_specs_pb2
+import smc.multiprocessing.networking.message_specs_pb2 as message_specs_pb2
+
from google.protobuf.internal.encoder import _VarintBytes
from google.protobuf.internal.decoder import _DecodeVarint32
import numpy as np
@@ -188,9 +189,9 @@ class DictPickleWithHeaderEncoderDecoder:
self.HEADERSIZE = 10
self.buffer = b""
- def dictToSerializedMsg(self, dict_msg : dict):
+ def dictToSerializedMsg(self, dict_msg: dict):
msg = pickle.dumps(dict_msg)
- return bytes(f"{len(msg)}:<{self.HEADERSIZE}", 'utf-8')+msg
+ return bytes(f"{len(msg)}:<{self.HEADERSIZE}", "utf-8") + msg
def what(self):
pass
diff --git a/python/smc/multiprocessing/process_manager.py b/python/smc/multiprocessing/process_manager.py
index 89757bc882fa50764a05aef12b5daae4b7217e93..315587b082242461a417c6722b4c2571aed17309 100644
--- a/python/smc/multiprocessing/process_manager.py
+++ b/python/smc/multiprocessing/process_manager.py
@@ -150,7 +150,7 @@ class ProcessManager:
# the key should be a string containing the command,
# and the value should be the data associated with the command,
# just to have some consistency
- def sendCommand(self, command: typing.Union[dict, np.ndarray]):
+ def sendCommand(self, command: typing.Union[dict, np.ndarray]) -> None:
"""
sendCommand
------------
@@ -182,7 +182,7 @@ class ProcessManager:
self.shared_command[:] = command[:]
self.shared_command_lock.release()
- def getData(self):
+ def getData(self) -> dict[str, typing.Any]:
if self.comm_direction < 3:
if not self.data_queue.empty():
self.latest_data = self.data_queue.get_nowait()
@@ -208,7 +208,7 @@ class ProcessManager:
# self.latest_data = self.encoder_decoder.serializedPb2MsgToDict(data_copy, self.msg_code)
return copy.deepcopy(self.latest_data)
- def terminateProcess(self):
+ def terminateProcess(self) -> None:
if self.comm_direction == 3:
self.shm_cmd.close()
self.shm_cmd.unlink()
diff --git a/python/smc/path_generation/path_math/cart_pulling_path_math.py b/python/smc/path_generation/path_math/cart_pulling_path_math.py
index bedff3a280b5637668c749a5ebd910ec48ea2ce5..499fc63baa86c3ea7ce00da1f31a6e8ac79a2a15 100644
--- a/python/smc/path_generation/path_math/cart_pulling_path_math.py
+++ b/python/smc/path_generation/path_math/cart_pulling_path_math.py
@@ -9,6 +9,13 @@ from argparse import Namespace
from collections import deque
+# NOTE: extremely inefficient.
+# there should be no copy-pasting of the whole path at every single point in time,
+# instead all the computations should be cached.
+# ideally you don't even cast the whole past_path into an array, but work on the queue,
+# and just do the smallest update.
+# this shouldn't be too difficult, it's just annoying. first we fix the logic,
+# then we improve efficiency.
def getCurrentPositionHandlebarInPastBasePath(
args: Namespace, p_base_current: np.ndarray, past_path2D_untimed: np.ndarray
) -> int:
diff --git a/python/smc/path_generation/path_math/path_to_trajectory.py b/python/smc/path_generation/path_math/path_to_trajectory.py
index dace0769317a4bd7b436e8702fd3529ee26559e4..15d9fdd5e5193d93aea0212cb6a312cb839adefc 100644
--- a/python/smc/path_generation/path_math/path_to_trajectory.py
+++ b/python/smc/path_generation/path_math/path_to_trajectory.py
@@ -53,7 +53,9 @@ def path2D_timed(args, path2D_untimed, max_base_v):
y_diff = y_i_plus_1 - y_i
y_diff = y_diff.reshape((-1, 1))
path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
+ print(path_length)
total_time = path_length / base_v
+ print(total_time)
# 2) we find the correspondence between s and time
# TODO: read from where it should be, but seba checked that it's 5 for some reason
# TODO THIS IS N IN PATH PLANNING, MAKE THIS A SHARED ARGUMENT
diff --git a/python/smc/path_generation/star_navigation/robot/bicycle.py b/python/smc/path_generation/star_navigation/robot/bicycle.py
index a2f4ec607e5633486bcd32eb54b3b93995e52545..19272209697c320b32fb7b5dabe47463debc8818 100644
--- a/python/smc/path_generation/star_navigation/robot/bicycle.py
+++ b/python/smc/path_generation/star_navigation/robot/bicycle.py
@@ -1,14 +1,23 @@
import numpy as np
-from robot import MobileRobot
+from ..robot import MobileRobot
class Bicycle(MobileRobot):
- def __init__(self, width, vel_min=None, vel_max=None, steer_max=None, name='robot'):
+ def __init__(self, width, vel_min=None, vel_max=None, steer_max=None, name="robot"):
self.vmax = vel_max[0]
x_min = [-np.inf] * 3 + [-steer_max]
x_max = [np.inf] * 3 + [steer_max]
- super().__init__(nu=2, nx=4, width=width, name=name, u_min=vel_min, u_max=vel_max, x_min=x_min, x_max=x_max)
+ super().__init__(
+ nu=2,
+ nx=4,
+ width=width,
+ name=name,
+ u_min=vel_min,
+ u_max=vel_max,
+ x_min=x_min,
+ x_max=x_max,
+ )
# [x,y of backwheel, orientation, steering angle]
# def f(self, x, u):
@@ -16,10 +25,13 @@ class Bicycle(MobileRobot):
# u[0] * np.sin(x[2]),
# u[0] * np.tan(u[1]) / self.width]
def f(self, x, u):
- return [u[0] * np.cos(x[2]),
- u[0] * np.sin(x[2]),
- u[0] * np.tan(u[1]) / self.width,
- u[1]]
+ return [
+ u[0] * np.cos(x[2]),
+ u[0] * np.sin(x[2]),
+ u[0] * np.tan(u[1]) / self.width,
+ u[1],
+ ]
+
# def f(self, x, u):
# return [u[0] * np.cos(x[2]) * np.cos(x[3]),
# u[0] * np.sin(x[2]) * np.cos(x[3]),
@@ -27,8 +39,10 @@ class Bicycle(MobileRobot):
# u[1]]
def h(self, x):
- return [x[0] + self.width/2 * np.cos(x[2]),
- x[1] + self.width/2 * np.sin(x[2])]
+ return [
+ x[0] + self.width / 2 * np.cos(x[2]),
+ x[1] + self.width / 2 * np.sin(x[2]),
+ ]
def vel_min(self):
return self.u_min
@@ -36,20 +50,27 @@ class Bicycle(MobileRobot):
def vel_max(self):
return self.u_max
- def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10, **kwargs):
+ def init_plot(self, ax=None, color="b", alpha=0.7, markersize=10, **kwargs):
h = super(Bicycle, self).init_plot(ax=ax, color=color, alpha=alpha)
- h += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color)
- h += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w')
+ h += ax.plot(
+ 0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color
+ )
+ h += ax.plot(
+ 0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5 * markersize, color="w"
+ )
h += ax.plot(0, 0, color=color, alpha=alpha)
return h
def update_plot(self, x, handles):
super(Bicycle, self).update_plot(x, handles)
handles[1].set_data(self.h(x))
- handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2)))
+ handles[1].set_marker((3, 0, np.rad2deg(x[2] - np.pi / 2)))
handles[1].set_markersize(handles[1].get_markersize())
handles[2].set_data(x[0], x[1])
- handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2)))
+ handles[2].set_marker((2, 0, np.rad2deg(x[2] - np.pi / 2)))
handles[2].set_markersize(handles[2].get_markersize())
- handles[3].set_data([x[0], x[0] + self.width * np.cos(x[2])],
- [x[1], x[1] + self.width * np.sin(x[2])])
\ No newline at end of file
+ handles[3].set_data(
+ [x[0], x[0] + self.width * np.cos(x[2])],
+ [x[1], x[1] + self.width * np.sin(x[2])],
+ )
+
diff --git a/python/smc/path_generation/star_navigation/robot/omnidirectional.py b/python/smc/path_generation/star_navigation/robot/omnidirectional.py
index f476facda106f46a8d81a2aaef6a0ab8de1f113e..72e4970b001242e9710e49d9af056d6d10e7ea9e 100644
--- a/python/smc/path_generation/star_navigation/robot/omnidirectional.py
+++ b/python/smc/path_generation/star_navigation/robot/omnidirectional.py
@@ -1,17 +1,23 @@
import numpy as np
-from robot import MobileRobot
+from ..robot import MobileRobot
class Omnidirectional(MobileRobot):
- def __init__(self, radius, vel_max, name='robot'):
+ def __init__(self, radius, vel_max, name="robot"):
vel_max = vel_max * np.ones(2)
self.vmax = float(np.linalg.norm(vel_max))
- super().__init__(nu=2, nx=2, radius=radius, name=name, u_min=(-vel_max).tolist(), u_max=(vel_max).tolist())
+ super().__init__(
+ nu=2,
+ nx=2,
+ radius=radius,
+ name=name,
+ u_min=(-vel_max).tolist(),
+ u_max=(vel_max).tolist(),
+ )
def f(self, x, u):
- return [u[0],
- u[1]]
+ return [u[0], u[1]]
def h(self, x):
return x[:2]
diff --git a/python/smc/path_generation/star_navigation/robot/unicycle.py b/python/smc/path_generation/star_navigation/robot/unicycle.py
index 5ae12267a93a733816c54fae8204b1b45128afc8..30aff50ae67b184fab838da2340c4c254a860712 100644
--- a/python/smc/path_generation/star_navigation/robot/unicycle.py
+++ b/python/smc/path_generation/star_navigation/robot/unicycle.py
@@ -1,5 +1,5 @@
import numpy as np
-from robot import MobileRobot
+from ..robot import MobileRobot
import matplotlib.pyplot as plt
@@ -13,38 +13,49 @@ def get_triangle_vertices(center, orientation, side_length):
# calculate the three vertices
vertex1 = (center[0] + x_offset, center[1] + y_offset)
- vertex2 = (center[0] + x_offset * np.cos(np.radians(120)) - y_offset * np.sin(np.radians(120)),
- center[1] + x_offset * np.sin(np.radians(120)) + y_offset * np.cos(np.radians(120)))
- vertex3 = (center[0] + x_offset * np.cos(np.radians(240)) - y_offset * np.sin(np.radians(240)),
- center[1] + x_offset * np.sin(np.radians(240)) + y_offset * np.cos(np.radians(240)))
+ vertex2 = (
+ center[0]
+ + x_offset * np.cos(np.radians(120))
+ - y_offset * np.sin(np.radians(120)),
+ center[1]
+ + x_offset * np.sin(np.radians(120))
+ + y_offset * np.cos(np.radians(120)),
+ )
+ vertex3 = (
+ center[0]
+ + x_offset * np.cos(np.radians(240))
+ - y_offset * np.sin(np.radians(240)),
+ center[1]
+ + x_offset * np.sin(np.radians(240))
+ + y_offset * np.cos(np.radians(240)),
+ )
return (vertex1, vertex2, vertex3)
+
class Unicycle(MobileRobot):
- def __init__(self, width, vel_min=None, vel_max=None, name='robot'):
+ def __init__(self, width, vel_min=None, vel_max=None, name="robot"):
self.vmax = vel_max[0]
- super().__init__(nu=2, nx=3, width=width, name=name, u_min=vel_min, u_max=vel_max)
+ super().__init__(
+ nu=2, nx=3, width=width, name=name, u_min=vel_min, u_max=vel_max
+ )
def f(self, x, u):
- return [u[0] * np.cos(x[2]), # vx
- u[0] * np.sin(x[2]), # vy
- u[1]] # wz
-
+ return [u[0] * np.cos(x[2]), u[0] * np.sin(x[2]), u[1]] # vx # vy # wz
def move(self, x, u, dt):
u_sat = np.clip(u, self.u_min, self.u_max)
x_next = x + np.array(self.f(x, u_sat)) * dt
x_next = np.clip(x_next, self.x_min, self.x_max)
- while x_next[2] > 2*np.pi:
+ while x_next[2] > 2 * np.pi:
x_next[2] -= 2 * np.pi
- while x_next[2] <= -0*np.pi:
+ while x_next[2] <= -0 * np.pi:
x_next[2] += 2 * np.pi
return x_next, u_sat
-
def h(self, x):
- return x[:2] # x, y
+ return x[:2] # x, y
def vel_min(self):
return self.u_min
@@ -52,17 +63,21 @@ class Unicycle(MobileRobot):
def vel_max(self):
return self.u_max
- def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10):
+ def init_plot(self, ax=None, color="b", alpha=0.7, markersize=10):
handles, ax = super(Unicycle, self).init_plot(ax=ax, color=color, alpha=alpha)
- handles += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color)
- handles += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w')
+ handles += ax.plot(
+ 0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color
+ )
+ handles += ax.plot(
+ 0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5 * markersize, color="w"
+ )
return handles, ax
def update_plot(self, x, handles):
super(Unicycle, self).update_plot(x, handles)
handles[1].set_data([x[0]], [x[1]])
- handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2)))
+ handles[1].set_marker((3, 0, np.rad2deg(x[2] - np.pi / 2)))
handles[1].set_markersize(handles[1].get_markersize())
handles[2].set_data([x[0]], [x[1]])
- handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2)))
+ handles[2].set_marker((2, 0, np.rad2deg(x[2] - np.pi / 2)))
handles[2].set_markersize(handles[2].get_markersize())
diff --git a/python/smc/path_generation/starworlds/obstacles/ellipse.py b/python/smc/path_generation/starworlds/obstacles/ellipse.py
index 17f216ea08bfd56748b3be73dbb0ffe8d9155481..ce09821eeaec2c0bf252f28231db8247c63a0a60 100644
--- a/python/smc/path_generation/starworlds/obstacles/ellipse.py
+++ b/python/smc/path_generation/starworlds/obstacles/ellipse.py
@@ -1,8 +1,8 @@
-from obstacles import StarshapedObstacle, Frame
+from ..obstacles import StarshapedObstacle, Frame
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
-from utils import is_ccw, tic, toc
+from ..utils import is_ccw, tic, toc
import shapely
@@ -15,12 +15,19 @@ class Ellipse(StarshapedObstacle):
if xr is None:
xr = np.zeros(2)
super().__init__(xr=xr, **kwargs)
- self.enclosing_ball_diameter = max(2*self._a[0], 2*self._a[1])
+ self.enclosing_ball_diameter = max(2 * self._a[0], 2 * self._a[1])
def copy(self, id, name):
- if (id == 'duplicate' or id == 'd'):
+ if id == "duplicate" or id == "d":
id = self.id()
- return Ellipse(id=id, name=name, a=self._a, xr=self._xr, n_pol=self._n_pol, motion_model=self._motion_model)
+ return Ellipse(
+ id=id,
+ name=name,
+ a=self._a,
+ xr=self._xr,
+ n_pol=self._n_pol,
+ motion_model=self._motion_model,
+ )
def dilated_obstacle(self, padding, id="new", name=None):
cp = self.copy(id, name)
@@ -36,16 +43,24 @@ class Ellipse(StarshapedObstacle):
def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
if ax is None:
- _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ _, ax = plt.subplots(subplot_kw={"aspect": "equal"})
# Default facecolor
if "fc" not in kwargs and "facecolor" not in kwargs:
- kwargs["fc"] = 'lightgrey'
+ kwargs["fc"] = "lightgrey"
line_handles = []
# Boundary
- line_handles += [patches.Ellipse(xy=[0, 0], width=2*self._a[0], height=2*self._a[1], angle=0, **kwargs)]
+ line_handles += [
+ patches.Ellipse(
+ xy=[0, 0],
+ width=2 * self._a[0],
+ height=2 * self._a[1],
+ angle=0,
+ **kwargs
+ )
+ ]
ax.add_patch(line_handles[-1])
# Reference point
- line_handles += ax.plot(*self._xr, '+', color='k') if show_reference else [None]
+ line_handles += ax.plot(*self._xr, "+", color="k") if show_reference else [None]
# Name
line_handles += [ax.text(*self._xr, self._name)] if show_name else [None]
return line_handles, ax
@@ -67,23 +82,50 @@ class Ellipse(StarshapedObstacle):
def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
- intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
- input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ intersect_obstacle = self.line_intersection(
+ [
+ self._xr,
+ self._xr
+ + 1.1
+ * self.enclosing_ball_diameter
+ * self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE),
+ ],
+ input_frame=Frame.OBSTACLE,
+ output_frame=Frame.OBSTACLE,
+ )
if not intersect_obstacle:
return None
if len(intersect_obstacle) == 1:
return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
else:
- if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
- return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ if np.linalg.norm(intersect_obstacle[0] - x_obstacle) < np.linalg.norm(
+ intersect_obstacle[1] - x_obstacle
+ ):
+ return self.transform(
+ intersect_obstacle[0], Frame.OBSTACLE, output_frame
+ )
else:
- return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+ return self.transform(
+ intersect_obstacle[1], Frame.OBSTACLE, output_frame
+ )
- def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
- b_obstacle = self.transform(x, input_frame, Frame.OBSTACLE) if x_is_boundary else self.boundary_mapping(x, input_frame, Frame.OBSTACLE)
+ def normal(
+ self,
+ x,
+ input_frame=Frame.GLOBAL,
+ output_frame=Frame.GLOBAL,
+ x_is_boundary=False,
+ ):
+ b_obstacle = (
+ self.transform(x, input_frame, Frame.OBSTACLE)
+ if x_is_boundary
+ else self.boundary_mapping(x, input_frame, Frame.OBSTACLE)
+ )
if b_obstacle is None:
return None
- n_obstacle = np.array([self._a2[1] * b_obstacle[0], self._a2[0] * b_obstacle[1]])
+ n_obstacle = np.array(
+ [self._a2[1] * b_obstacle[0], self._a2[0] * b_obstacle[1]]
+ )
n_obstacle = n_obstacle / np.linalg.norm(n_obstacle)
return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
@@ -92,18 +134,25 @@ class Ellipse(StarshapedObstacle):
loc = (x_obstacle[0] / self._a[0]) ** 2 + (x_obstacle[1] / self._a[1]) ** 2 - 1
return loc
- def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ def line_intersection(
+ self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL
+ ):
# Transform line points to left/right points in obstacle ellipse coordinates
l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
- l_left_obstacle = l0_obstacle if l0_obstacle[0] < l1_obstacle[0] else l1_obstacle
- l_right_obstacle = l1_obstacle if l0_obstacle[0] < l1_obstacle[0] else l0_obstacle
+ l_left_obstacle = (
+ l0_obstacle if l0_obstacle[0] < l1_obstacle[0] else l1_obstacle
+ )
+ l_right_obstacle = (
+ l1_obstacle if l0_obstacle[0] < l1_obstacle[0] else l0_obstacle
+ )
-
- vertical_line = abs(l_right_obstacle[0]-l_left_obstacle[0]) < 1e-4
+ vertical_line = abs(l_right_obstacle[0] - l_left_obstacle[0]) < 1e-4
if not vertical_line:
# Line parameters
- m = (l_right_obstacle[1] - l_left_obstacle[1]) / (l_right_obstacle[0] - l_left_obstacle[0])
+ m = (l_right_obstacle[1] - l_left_obstacle[1]) / (
+ l_right_obstacle[0] - l_left_obstacle[0]
+ )
c = l_left_obstacle[1] - m * l_left_obstacle[0]
vertical_line = abs(m) > 100
@@ -112,16 +161,32 @@ class Ellipse(StarshapedObstacle):
if l_right_obstacle[0] < -self._a[0] or l_right_obstacle[0] > self._a[0]:
return []
- l_top_obstacle = l_right_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_left_obstacle
- l_bottom_obstacle = l_left_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_right_obstacle
- x_intersect_top_obstacle, x_intersect_bottom_obstacle = np.array([0, self._a[1]]), np.array([0, -self._a[1]])
- x_intersect_top = self.transform(x_intersect_top_obstacle, Frame.OBSTACLE, output_frame)
- x_intersect_bottom = self.transform(x_intersect_bottom_obstacle, Frame.OBSTACLE, output_frame)
+ l_top_obstacle = (
+ l_right_obstacle
+ if l_right_obstacle[1] > l_left_obstacle[1]
+ else l_left_obstacle
+ )
+ l_bottom_obstacle = (
+ l_left_obstacle
+ if l_right_obstacle[1] > l_left_obstacle[1]
+ else l_right_obstacle
+ )
+ x_intersect_top_obstacle, x_intersect_bottom_obstacle = np.array(
+ [0, self._a[1]]
+ ), np.array([0, -self._a[1]])
+ x_intersect_top = self.transform(
+ x_intersect_top_obstacle, Frame.OBSTACLE, output_frame
+ )
+ x_intersect_bottom = self.transform(
+ x_intersect_bottom_obstacle, Frame.OBSTACLE, output_frame
+ )
if l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
return [x_intersect_top, x_intersect_bottom]
elif l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= self._a[1]:
return [x_intersect_top]
- elif l_top_obstacle[1] >= -self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ elif (
+ l_top_obstacle[1] >= -self._a[1] and l_bottom_obstacle[1] <= -self._a[1]
+ ):
return [x_intersect_bottom]
else:
return []
@@ -140,18 +205,28 @@ class Ellipse(StarshapedObstacle):
x_intersect_obstacle = np.array([tmp_x, m * tmp_x + c])
return [self.transform(x_intersect_obstacle, Frame.OBSTACLE, output_frame)]
else:
- in_sqrt = kx ** 2 / (4 * kx2 ** 2) - k1 / kx2
+ in_sqrt = kx**2 / (4 * kx2**2) - k1 / kx2
if np.isclose(in_sqrt, 0):
in_sqrt = 0
tmp_x = -kx / (2 * kx2) - np.sqrt(in_sqrt)
x_intersect_left_obstacle = np.array([tmp_x, m * tmp_x + c])
tmp_x = -kx / (2 * kx2) + np.sqrt(in_sqrt)
x_intersect_right_obstacle = np.array([tmp_x, m * tmp_x + c])
- x_intersect_left = self.transform(x_intersect_left_obstacle, Frame.OBSTACLE, output_frame)
- x_intersect_right = self.transform(x_intersect_right_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_left = self.transform(
+ x_intersect_left_obstacle, Frame.OBSTACLE, output_frame
+ )
+ x_intersect_right = self.transform(
+ x_intersect_right_obstacle, Frame.OBSTACLE, output_frame
+ )
- if l_right_obstacle[0] < x_intersect_left_obstacle[0] or x_intersect_right_obstacle[0] < l_left_obstacle[0] or \
- x_intersect_left_obstacle[0] < l_left_obstacle[0] < l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ if (
+ l_right_obstacle[0] < x_intersect_left_obstacle[0]
+ or x_intersect_right_obstacle[0] < l_left_obstacle[0]
+ or x_intersect_left_obstacle[0]
+ < l_left_obstacle[0]
+ < l_right_obstacle[0]
+ < x_intersect_right_obstacle[0]
+ ):
return []
elif x_intersect_left_obstacle[0] < l_left_obstacle[0]:
return [x_intersect_right]
@@ -170,8 +245,8 @@ class Ellipse(StarshapedObstacle):
a2 = self._a**2
vertical_tangent = abs(a2[0] - px**2) < 1e-5
if vertical_tangent:
- m2 = (py**2-a2[1]) / (2*px*py)
- x2 = (px * m2 ** 2 - py * m2) / (a2[1] / a2[0] + m2 ** 2)
+ m2 = (py**2 - a2[1]) / (2 * px * py)
+ x2 = (px * m2**2 - py * m2) / (a2[1] / a2[0] + m2**2)
y2 = m2 * (x2 - px) + py
tp1_obstacle = np.array([px, 0])
tp2_obstacle = np.array([x2, y2])
@@ -179,7 +254,7 @@ class Ellipse(StarshapedObstacle):
tmp = px**2 - a2[0]
c1 = (px * py) / tmp
c2 = (a2[1] - py**2) / tmp
- tmp = np.sqrt(c1**2+c2)
+ tmp = np.sqrt(c1**2 + c2)
m1, m2 = c1 + tmp, c1 - tmp
tmp = a2[1] / a2[0]
m1_sqr = m1**2
@@ -212,6 +287,6 @@ class Ellipse(StarshapedObstacle):
def _compute_polygon_representation(self):
# logprint(str(self) + ": " + str(self._polygon), 0)
t = np.linspace(0, 2 * np.pi, self._n_pol, endpoint=False)
- a = self._a + 1e-3 # Add offset to adjust for polygon approximation
+ a = self._a + 1e-3 # Add offset to adjust for polygon approximation
polygon = np.vstack((a[0] * np.cos(t), a[1] * np.sin(t))).T
self._polygon = shapely.geometry.Polygon(polygon)
diff --git a/python/smc/path_generation/starworlds/obstacles/obstacle.py b/python/smc/path_generation/starworlds/obstacles/obstacle.py
index 7789611537fe4a9621377a6521a8c1d6ae239f7d..262fb1b7fca1c8de9e48013754e68c5693a68f12 100644
--- a/python/smc/path_generation/starworlds/obstacles/obstacle.py
+++ b/python/smc/path_generation/starworlds/obstacles/obstacle.py
@@ -2,7 +2,7 @@ from abc import ABC, abstractmethod
import numpy as np
import shapely
from shapely import affinity as sh_affinity
-from utils import affine_transform
+from ..utils import affine_transform
from copy import deepcopy
from enum import Enum
@@ -16,30 +16,43 @@ class Frame(Enum):
class Obstacle(ABC):
- """ Abstract base class of obstacles
- """
+ """Abstract base class of obstacles"""
+
id_counter = 0
# obs_id <0: temp object, obs_id=0: new object, obs_id>0: existing object with id #obs_id
- def __init__(self, motion_model=None, is_convex=None, is_starshaped=None, id='new', name=None, compute_polygon=False):
+ def __init__(
+ self,
+ motion_model=None,
+ is_convex=None,
+ is_starshaped=None,
+ id="new",
+ name=None,
+ compute_polygon=False,
+ ):
self._id = None
self._is_convex = is_convex
self._is_starshaped = is_starshaped
# Pose of local frame in global frame
- self._motion_model = motion_model # if motion_model is not None else mm.Static([0., 0.], 0.)
+ self._motion_model = (
+ motion_model # if motion_model is not None else mm.Static([0., 0.], 0.)
+ )
# Initialize id and name
self._set_id_name(id, name)
self._polygon = None # Polygon in obstacle frame
self._polygon_global = None # Polygon in global frame if static obstacle
- self._polygon_global_pose = None # Obstacle pose corresponding to global polygon
+ self._polygon_global_pose = (
+ None # Obstacle pose corresponding to global polygon
+ )
if compute_polygon:
self._compute_global_polygon_representation()
- def __str__(self): return self._name
+ def __str__(self):
+ return self._name
- def copy(self, id='temporary', name=None):
+ def copy(self, id="temporary", name=None):
ob = deepcopy(self)
- if not (id == 'duplicate' or id == 'd'):
+ if not (id == "duplicate" or id == "d"):
ob._set_id_name(id, name)
return ob
@@ -51,7 +64,7 @@ class Obstacle(ABC):
def rot(self, output_frame=Frame.GLOBAL):
if output_frame == Frame.OBSTACLE or self._motion_model is None:
- return 0.
+ return 0.0
if output_frame == Frame.GLOBAL:
return self._motion_model.rot()
@@ -62,13 +75,14 @@ class Obstacle(ABC):
return True if self.point_location(x, input_frame) > 0 else False
def boundary_point(self, x, input_frame=Frame.GLOBAL):
- return np.isclose(self.point_location(x, input_frame), 0.)
+ return np.isclose(self.point_location(x, input_frame), 0.0)
def move(self, dt):
if self._motion_model is not None:
self._motion_model.move(self, dt)
- def id(self): return self._id
+ def id(self):
+ return self._id
def polygon(self, output_frame=Frame.GLOBAL):
if self._polygon is None:
@@ -81,9 +95,20 @@ class Obstacle(ABC):
if not current_pose == self._polygon_global_pose:
self._polygon_global_pose = current_pose
c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
- trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
- affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
- self._polygon_global = sh_affinity.affine_transform(self._polygon, affinity_matrix)
+ trans_matrix = np.array(
+ [[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]]
+ )
+ affinity_matrix = [
+ trans_matrix[0, 0],
+ trans_matrix[0, 1],
+ trans_matrix[1, 0],
+ trans_matrix[1, 1],
+ trans_matrix[0, 2],
+ trans_matrix[1, 2],
+ ]
+ self._polygon_global = sh_affinity.affine_transform(
+ self._polygon, affinity_matrix
+ )
return self._polygon_global
else:
raise Frame.InvalidFrameError
@@ -131,10 +156,10 @@ class Obstacle(ABC):
# ------------ Private methods ------------ #
def _set_id_name(self, id, name=None):
- if id == 'new' or id == 'n':
+ if id == "new" or id == "n":
Obstacle.id_counter += 1
self._id = Obstacle.id_counter
- elif id == 'temporary' or id == 'temp' or id == 't':
+ elif id == "temporary" or id == "temp" or id == "t":
self._id = None
elif isinstance(id, int) and 0 < id <= Obstacle.id_counter:
self._id = id
@@ -149,7 +174,9 @@ class Obstacle(ABC):
def _rotate_global2obstacle(self, x_global):
rot = self._motion_model.rot()
- return affine_transform(x_global, rotation=rot, translation=[0, 0], inverse=True)
+ return affine_transform(
+ x_global, rotation=rot, translation=[0, 0], inverse=True
+ )
def _transform_obstacle2global(self, x_obstacle):
pos, rot = (self._motion_model.pos(), self._motion_model.rot())
@@ -163,13 +190,21 @@ class Obstacle(ABC):
self._compute_polygon_representation()
if self._motion_model is None:
self._polygon_global = self._polygon
- if self._motion_model.__class__.__name__ == 'Static':
+ if self._motion_model.__class__.__name__ == "Static":
pos, rot = (self._motion_model.pos(), self._motion_model.rot())
c, s = np.cos(rot), np.sin(rot)
trans_matrix = np.array([[c, -s, pos[0]], [s, c, pos[1]], [0, 0, 1]])
- affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1],
- trans_matrix[0, 2], trans_matrix[1, 2]]
- self._polygon_global = shapely.affinity.affine_transform(self._polygon, affinity_matrix)
+ affinity_matrix = [
+ trans_matrix[0, 0],
+ trans_matrix[0, 1],
+ trans_matrix[1, 0],
+ trans_matrix[1, 1],
+ trans_matrix[0, 2],
+ trans_matrix[1, 2],
+ ]
+ self._polygon_global = shapely.affinity.affine_transform(
+ self._polygon, affinity_matrix
+ )
# ------------ Abstract methods ------------ #
@abstractmethod
@@ -185,7 +220,9 @@ class Obstacle(ABC):
pass
@abstractmethod
- def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ def line_intersection(
+ self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL
+ ):
pass
@abstractmethod
diff --git a/python/smc/path_generation/starworlds/obstacles/polygon.py b/python/smc/path_generation/starworlds/obstacles/polygon.py
index d8ec63a62b891136497967a50f588d08dd1b8343..97ba8d16cb2d6bf3cd46cb2095154612f8711ce3 100644
--- a/python/smc/path_generation/starworlds/obstacles/polygon.py
+++ b/python/smc/path_generation/starworlds/obstacles/polygon.py
@@ -1,5 +1,5 @@
-from obstacles import Obstacle, Frame
-from utils import is_cw, is_ccw, is_collinear, tic, toc
+from ..obstacles import Obstacle, Frame
+from ..utils import is_cw, is_ccw, is_collinear, tic, toc
import shapely
import numpy as np
import matplotlib.pyplot as plt
@@ -19,11 +19,11 @@ class Polygon(Obstacle):
def init_plot(self, ax=None, show_name=False, **kwargs):
if ax is None:
- _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ _, ax = plt.subplots(subplot_kw={"aspect": "equal"})
if "fc" not in kwargs and "facecolor" not in kwargs:
- kwargs["fc"] = 'lightgrey'
- if 'show_reference' in kwargs:
- del kwargs['show_reference']
+ kwargs["fc"] = "lightgrey"
+ if "show_reference" in kwargs:
+ del kwargs["show_reference"]
line_handles = []
# Boundary
line_handles += [patches.Polygon(np.random.rand(3, 2), **kwargs)]
@@ -70,29 +70,49 @@ class Polygon(Obstacle):
return 0
return 1
- def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ def line_intersection(
+ self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL
+ ):
l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
- intersection_points_shapely = shapely.geometry.LineString([l0_obstacle, l1_obstacle]).intersection(self._polygon.exterior)
+ intersection_points_shapely = shapely.geometry.LineString(
+ [l0_obstacle, l1_obstacle]
+ ).intersection(self._polygon.exterior)
if intersection_points_shapely.is_empty:
return []
- if intersection_points_shapely.geom_type == 'Point':
- intersection_points_obstacle = [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
- elif intersection_points_shapely.geom_type == 'MultiPoint':
- intersection_points_obstacle = [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
- elif intersection_points_shapely.geom_type == 'LineString':
- intersection_points_obstacle = [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
- elif intersection_points_shapely.geom_type == 'MultiLineString':
- intersection_points_obstacle = [np.array([ip[0], ip[1]]) for line in intersection_points_shapely.geoms for ip in line.coords]
+ if intersection_points_shapely.geom_type == "Point":
+ intersection_points_obstacle = [
+ np.array([intersection_points_shapely.x, intersection_points_shapely.y])
+ ]
+ elif intersection_points_shapely.geom_type == "MultiPoint":
+ intersection_points_obstacle = [
+ np.array([p.x, p.y]) for p in intersection_points_shapely.geoms
+ ]
+ elif intersection_points_shapely.geom_type == "LineString":
+ intersection_points_obstacle = [
+ np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords
+ ]
+ elif intersection_points_shapely.geom_type == "MultiLineString":
+ intersection_points_obstacle = [
+ np.array([ip[0], ip[1]])
+ for line in intersection_points_shapely.geoms
+ for ip in line.coords
+ ]
else:
print(intersection_points_shapely)
- return [self.transform(ip, Frame.OBSTACLE, output_frame) for ip in intersection_points_obstacle]
+ return [
+ self.transform(ip, Frame.OBSTACLE, output_frame)
+ for ip in intersection_points_obstacle
+ ]
def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
t0 = tic()
- phi = np.arctan2(self.circular_vertices[:, 1] - x_obstacle[1], self.circular_vertices[:, 0] - x_obstacle[0])
+ phi = np.arctan2(
+ self.circular_vertices[:, 1] - x_obstacle[1],
+ self.circular_vertices[:, 0] - x_obstacle[0],
+ )
phi[phi < 0] += 2 * np.pi
t1 = toc(t0)
t0 = tic()
@@ -106,9 +126,9 @@ class Polygon(Obstacle):
phi_decrease_idcs = np.flatnonzero(phi_decrease_idcs)
phi_increase_idcs = np.flatnonzero(phi_increase_idcs)
for i in phi_decrease_idcs:
- phi[i+1:] -= 2*np.pi
+ phi[i + 1 :] -= 2 * np.pi
for i in phi_increase_idcs:
- phi[i+1:] += 2*np.pi
+ phi[i + 1 :] += 2 * np.pi
t4 = toc(t0)
t0 = tic()
@@ -118,7 +138,7 @@ class Polygon(Obstacle):
if abs(phi[0] - phi[-1]) > 0.00001:
# Interior point
return []
- if (phi[i_max] - phi[i_min]) >= 2*np.pi:
+ if (phi[i_max] - phi[i_min]) >= 2 * np.pi:
# Blocked exterior point
return []
t5 = toc(t0)
@@ -143,15 +163,25 @@ class Polygon(Obstacle):
i = 0
N = v.shape[0]
# Make sure first vertice is not collinear
- while is_collinear(v[i-1, :], v[i, :], v[(i+1) % N, :]):
+ while is_collinear(v[i - 1, :], v[i, :], v[(i + 1) % N, :]):
i += 1
if i > N:
raise RuntimeError("Bad polygon shape. All vertices collinear")
# All vertices must be either cw or ccw when iterating through for convexity
- if is_cw(v[i-1, :], v[i, :], v[i+1, :]):
- self._is_convex = not any([is_ccw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+ if is_cw(v[i - 1, :], v[i, :], v[i + 1, :]):
+ self._is_convex = not any(
+ [
+ is_ccw(v[j - 1, :], v[j, :], v[(j + 1) % N, :])
+ for j in range(v.shape[0])
+ ]
+ )
else:
- self._is_convex = not any([is_cw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+ self._is_convex = not any(
+ [
+ is_cw(v[j - 1, :], v[j, :], v[(j + 1) % N, :])
+ for j in range(v.shape[0])
+ ]
+ )
# Not needed
def _compute_polygon_representation(self):
diff --git a/python/smc/path_generation/starworlds/obstacles/starshaped_obstacle.py b/python/smc/path_generation/starworlds/obstacles/starshaped_obstacle.py
index 122ab8f02c24f2b1c0d57754353c639f4d5a3fae..85cc2d8d32b7a5b9afffda815edbb061276a84df 100644
--- a/python/smc/path_generation/starworlds/obstacles/starshaped_obstacle.py
+++ b/python/smc/path_generation/starworlds/obstacles/starshaped_obstacle.py
@@ -1,6 +1,6 @@
from abc import abstractmethod
import numpy as np
-from obstacles import Obstacle, Frame
+from ..obstacles import Obstacle, Frame
from shapely import affinity as sh_affinity
@@ -25,8 +25,12 @@ class StarshapedObstacle(Obstacle):
else:
self._xr = new_xr
- def reference_direction(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
- dir = self.transform(x, input_frame, output_frame) - self.transform(self._xr, Frame.OBSTACLE, output_frame)
+ def reference_direction(
+ self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL
+ ):
+ dir = self.transform(x, input_frame, output_frame) - self.transform(
+ self._xr, Frame.OBSTACLE, output_frame
+ )
if not np.any(dir):
print("reference_direction for xr is not defined")
return dir / np.linalg.norm(dir, axis=x.ndim - 1)
@@ -34,14 +38,25 @@ class StarshapedObstacle(Obstacle):
# interior point: <1. exterior point: >1. boundary point: 1.
def distance_function(self, x, input_frame=Frame.GLOBAL):
x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
- dist_func = (np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1) / (
- np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
- - self._xr, axis=x.ndim - 1))) ** 2
+ dist_func = (
+ np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1)
+ / (
+ np.linalg.norm(
+ self.boundary_mapping(
+ x_obstacle,
+ input_frame=Frame.OBSTACLE,
+ output_frame=Frame.OBSTACLE,
+ )
+ - self._xr,
+ axis=x.ndim - 1,
+ )
+ )
+ ) ** 2
return dist_func
# interior point: <0. exterior point: >0. boundary point: 0.
def point_location(self, x, input_frame=Frame.GLOBAL):
- return np.sign(self.distance_function(x, input_frame)-1.)
+ return np.sign(self.distance_function(x, input_frame) - 1.0)
def kernel(self, output_frame=Frame.GLOBAL):
if self._kernel is None:
@@ -53,9 +68,20 @@ class StarshapedObstacle(Obstacle):
if not current_pose == self._kernel_global_pose:
self._kernel_global_pose = current_pose
c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
- trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
- affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
- self._kernel_global = sh_affinity.affine_transform(self._kernel, affinity_matrix)
+ trans_matrix = np.array(
+ [[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]]
+ )
+ affinity_matrix = [
+ trans_matrix[0, 0],
+ trans_matrix[0, 1],
+ trans_matrix[1, 0],
+ trans_matrix[1, 1],
+ trans_matrix[0, 2],
+ trans_matrix[1, 2],
+ ]
+ self._kernel_global = sh_affinity.affine_transform(
+ self._kernel, affinity_matrix
+ )
return self._kernel_global
else:
raise Frame.InvalidFrameError
@@ -64,12 +90,20 @@ class StarshapedObstacle(Obstacle):
if self._motion_model is None:
return np.zeros(2)
omega = self._motion_model.rot_vel()
- lin_vel_omega = np.cross(np.hstack(([0, 0], omega)), np.hstack((self.reference_direction(x), 0)))[:2]
+ lin_vel_omega = np.cross(
+ np.hstack(([0, 0], omega)), np.hstack((self.reference_direction(x), 0))
+ )[:2]
return self._motion_model.lin_vel() + lin_vel_omega
# ------------ Abstract methods ------------ #
@abstractmethod
- def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ def normal(
+ self,
+ x,
+ input_frame=Frame.GLOBAL,
+ output_frame=Frame.GLOBAL,
+ x_is_boundary=False,
+ ):
pass
@abstractmethod
diff --git a/python/smc/path_generation/starworlds/obstacles/starshaped_polygon.py b/python/smc/path_generation/starworlds/obstacles/starshaped_polygon.py
index 85eba3ce218671de85d625c612a6fa9494377efe..d17a1282c729d60cbb9e4c9f27bfbce2ae1708de 100644
--- a/python/smc/path_generation/starworlds/obstacles/starshaped_polygon.py
+++ b/python/smc/path_generation/starworlds/obstacles/starshaped_polygon.py
@@ -1,8 +1,8 @@
-from obstacles import Frame, StarshapedObstacle, Polygon
+from ..obstacles import Frame, StarshapedObstacle, Polygon
import matplotlib.pyplot as plt
import numpy as np
import shapely
-from utils import is_cw, is_ccw, is_collinear, orientation_val, get_intersection, is_between, tic, toc
+from ..utils import is_cw, is_ccw, is_collinear, orientation_val, get_intersection, is_between, tic, toc
class StarshapedPolygon(Polygon, StarshapedObstacle):
diff --git a/python/smc/path_generation/starworlds/obstacles/starshaped_primitive_combination.py b/python/smc/path_generation/starworlds/obstacles/starshaped_primitive_combination.py
index 6fdf2cd7ba907711261851bd22c60a6ab5d1b95d..339fe235c37d363b93f46ed7be9f48287d5315ca 100644
--- a/python/smc/path_generation/starworlds/obstacles/starshaped_primitive_combination.py
+++ b/python/smc/path_generation/starworlds/obstacles/starshaped_primitive_combination.py
@@ -1,9 +1,10 @@
import shapely
import numpy as np
-from obstacles import Frame, StarshapedObstacle, StarshapedPolygon
-from utils import is_ccw, is_cw, draw_shapely_polygon
+from ..obstacles import Frame, StarshapedObstacle, StarshapedPolygon
+from ..utils import is_ccw, is_cw, draw_shapely_polygon
import matplotlib.pyplot as plt
+
# Note: Local == Global frame
class StarshapedPrimitiveCombination(StarshapedObstacle):
@@ -25,8 +26,10 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
pass
def point_location(self, x, input_frame=Frame.GLOBAL):
- locs = [obs.point_location(x, input_frame=Frame.GLOBAL) for obs in self._obstacle_cluster] + \
- [self._hull_cluster_point_location(x)]
+ locs = [
+ obs.point_location(x, input_frame=Frame.GLOBAL)
+ for obs in self._obstacle_cluster
+ ] + [self._hull_cluster_point_location(x)]
if any([l < 0 for l in locs]):
# Interior point
return -1
@@ -36,7 +39,9 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
# Exterior point
return 1
- def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ def line_intersection(
+ self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL
+ ):
intersection_points = []
for o in self._obstacle_cluster:
intersection_points += o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
@@ -49,30 +54,51 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
for obs in self._obstacle_cluster:
tp_candidates += obs.tangent_points(x, Frame.GLOBAL, Frame.GLOBAL)
for i in range(len(tp_candidates)):
- if all([is_ccw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]) or \
- all([is_cw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]):
+ if all(
+ [
+ is_ccw(x, tp_candidates[i], tp_candidates[j])
+ for j in range(len(tp_candidates))
+ if j is not i
+ ]
+ ) or all(
+ [
+ is_cw(x, tp_candidates[i], tp_candidates[j])
+ for j in range(len(tp_candidates))
+ if j is not i
+ ]
+ ):
tp += [tp_candidates[i]]
return tp
def _compute_kernel(self):
- self._kernel = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").kernel()
+ self._kernel = StarshapedPolygon(
+ self.polygon(), xr=self.xr(), id="temp"
+ ).kernel()
def _check_convexity(self):
- self._is_convex = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").is_convex()
+ self._is_convex = StarshapedPolygon(
+ self.polygon(), xr=self.xr(), id="temp"
+ ).is_convex()
def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
# intersection_points = [p for ps in self.line_intersection([self._xr, self._xr+10*(x-self._xr)]) for p in ps]
- intersection_points = self.line_intersection([self._xr, self._xr+10*(x-self._xr)])
+ intersection_points = self.line_intersection(
+ [self._xr, self._xr + 10 * (x - self._xr)]
+ )
if not intersection_points:
return None
- dist_intersection_points = [np.linalg.norm(ip - self._xr) for ip in intersection_points]
+ dist_intersection_points = [
+ np.linalg.norm(ip - self._xr) for ip in intersection_points
+ ]
return intersection_points[np.argmax(dist_intersection_points)]
def vel_intertial_frame(self, x):
boundary_obs_idx = 0
max_dist = -1
- for i, ps in enumerate(self.line_intersection([self._xr, self._xr+10*(x-self._xr)])):
- o_intersection_dist = max([np.linalg.norm(p-self._xr) for p in ps] + [-1])
+ for i, ps in enumerate(
+ self.line_intersection([self._xr, self._xr + 10 * (x - self._xr)])
+ ):
+ o_intersection_dist = max([np.linalg.norm(p - self._xr) for p in ps] + [-1])
if o_intersection_dist > max_dist:
boundary_obs_idx = i
max_dist = o_intersection_dist
@@ -80,13 +106,22 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
boundary_obs_idx -= len(self._obstacle_cluster)
return self._obstacle_cluster[boundary_obs_idx].vel_intertial_frame(x)
- def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='weigthed_polygon_approx'):
- if type == 'sub_normal':
+ def normal(
+ self,
+ x,
+ input_frame=Frame.GLOBAL,
+ output_frame=Frame.GLOBAL,
+ x_is_boundary=False,
+ type="weigthed_polygon_approx",
+ ):
+ if type == "sub_normal":
boundary_obs_idx = 0
max_dist = -1
- line = [self._xr, self._xr+10*(x-self._xr)]
+ line = [self._xr, self._xr + 10 * (x - self._xr)]
for i, o in enumerate(self._obstacle_cluster):
- intersection_points = o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ intersection_points = o.line_intersection(
+ line, Frame.GLOBAL, Frame.GLOBAL
+ )
for p in intersection_points:
p_dist = np.linalg.norm(p - self._xr)
if p_dist > max_dist:
@@ -101,11 +136,18 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
max_dist = p_dist
boundary_obs_idx = len(self._obstacle_cluster)
if boundary_obs_idx < len(self._obstacle_cluster):
- return self._obstacle_cluster[boundary_obs_idx].normal(x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL)
+ return self._obstacle_cluster[boundary_obs_idx].normal(
+ x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL
+ )
else:
boundary_obs_idx -= len(self._obstacle_cluster)
hull_vertices = np.array(self._hull_cluster.exterior.coords[:-1])
- vertex_angles = np.array([np.arctan2(v[1] - self._xr[1], v[0] - self._xr[0]) for v in hull_vertices]).flatten()
+ vertex_angles = np.array(
+ [
+ np.arctan2(v[1] - self._xr[1], v[0] - self._xr[0])
+ for v in hull_vertices
+ ]
+ ).flatten()
idcs = np.argsort(vertex_angles)
vertex_angles = vertex_angles[idcs]
hull_vertices = hull_vertices[idcs, :]
@@ -117,11 +159,15 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
# Adjust for circular self.vertices (self.vertices[0] == self.vertices[-1])
if v_idx == 0:
v_idx = -1
- n = np.array([hull_vertices[v_idx, 1] - hull_vertices[v_idx - 1, 1],
- hull_vertices[v_idx - 1, 0] - hull_vertices[v_idx, 0]])
+ n = np.array(
+ [
+ hull_vertices[v_idx, 1] - hull_vertices[v_idx - 1, 1],
+ hull_vertices[v_idx - 1, 0] - hull_vertices[v_idx, 0],
+ ]
+ )
n /= np.linalg.norm(n)
return n
- elif type == 'polygon_approx' or type == 'weigthed_polygon_approx':
+ elif type == "polygon_approx" or type == "weigthed_polygon_approx":
if self.vertices is None:
self._update_vertex_angles()
angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
@@ -130,17 +176,30 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
if v_idx == self.vertices.shape[0]:
v_idx = 0
- if type == 'polygon_approx':
- n = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1],
- self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ if type == "polygon_approx":
+ n = np.array(
+ [
+ self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1],
+ self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0],
+ ]
+ )
else:
- edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]],
- self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
- edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
- e[0][0] - e[1][0]] for e in edge_neighbors])
- edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
- shapely.geometry.Point(x))[0].coords[0] for e in
- edge_neighbors]
+ edge_neighbors = [
+ (
+ self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]],
+ self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]],
+ )
+ for i in range(3)
+ ]
+ edge_neighbors_normal = np.array(
+ [[e[1][1] - e[0][1], e[0][0] - e[1][0]] for e in edge_neighbors]
+ )
+ edge_closest = [
+ shapely.ops.nearest_points(
+ shapely.geometry.LineString(e), shapely.geometry.Point(x)
+ )[0].coords[0]
+ for e in edge_neighbors
+ ]
dist = [np.linalg.norm(np.array(e) - x) for e in edge_closest]
w = np.array([1 / (d + 1e-10) for d in dist])
@@ -153,12 +212,12 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
super().set_xr(xr, input_frame, safe_set)
self._update_vertex_angles()
-
+
def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
if ax is None:
- _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ _, ax = plt.subplots(subplot_kw={"aspect": "equal"})
if "fc" not in kwargs and "facecolor" not in kwargs:
- kwargs["fc"] = 'lightgrey'
+ kwargs["fc"] = "lightgrey"
line_handles = []
@@ -166,7 +225,9 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
line_handles += lh
# Reference point
- line_handles += ax.plot(*self.xr(), '+', color='k') if show_reference else [None]
+ line_handles += (
+ ax.plot(*self.xr(), "+", color="k") if show_reference else [None]
+ )
# Name
line_handles += [ax.text(*self.xr(), self._name)] if show_name else [None]
return line_handles, ax
@@ -196,15 +257,22 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
intersection_points_shapely = line_sh.intersection(self._hull_cluster.exterior)
if intersection_points_shapely.is_empty:
return []
- if intersection_points_shapely.geom_type == 'Point':
- return [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
- elif intersection_points_shapely.geom_type == 'MultiPoint':
+ if intersection_points_shapely.geom_type == "Point":
+ return [
+ np.array([intersection_points_shapely.x, intersection_points_shapely.y])
+ ]
+ elif intersection_points_shapely.geom_type == "MultiPoint":
return [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
- elif intersection_points_shapely.geom_type == 'LineString':
- return [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
- elif intersection_points_shapely.geom_type == 'MultiLineString':
- return [np.array([l[0], l[1]]) for line in intersection_points_shapely.geoms for
- l in line.coords]
+ elif intersection_points_shapely.geom_type == "LineString":
+ return [
+ np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords
+ ]
+ elif intersection_points_shapely.geom_type == "MultiLineString":
+ return [
+ np.array([l[0], l[1]])
+ for line in intersection_points_shapely.geoms
+ for l in line.coords
+ ]
else:
print("[_hull_cluster_line_intersections]: Shapely geom_type not covered!")
print(intersection_points_shapely)
@@ -212,12 +280,16 @@ class StarshapedPrimitiveCombination(StarshapedObstacle):
def _update_vertex_angles(self):
self.vertices = np.array(self._polygon.exterior.coords[:-1])
self.circular_vertices = np.array(self._polygon.exterior.coords)
- self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ self.vertex_angles = np.arctan2(
+ self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0]
+ )
idcs = np.argsort(self.vertex_angles)
self.vertex_angles = self.vertex_angles[idcs]
self.vertices = self.vertices[idcs, :]
self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
- self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+ self.vertex_angles = np.hstack(
+ (self.vertex_angles, self.vertex_angles[0] + 2 * np.pi)
+ )
def _compute_polygon_representation(self):
obs_pol = [obs.polygon() for obs in self._obstacle_cluster]
diff --git a/python/smc/path_generation/starworlds/starshaped_hull/cluster_and_starify.py b/python/smc/path_generation/starworlds/starshaped_hull/cluster_and_starify.py
index 2e825e86581a3bbecb805d3ea1351a1a6a5c95ed..85ba23dce36ce6dabe1744f658edca314a2c0181 100644
--- a/python/smc/path_generation/starworlds/starshaped_hull/cluster_and_starify.py
+++ b/python/smc/path_generation/starworlds/starshaped_hull/cluster_and_starify.py
@@ -1,9 +1,9 @@
import shapely
import numpy as np
-from obstacles import Frame, StarshapedPrimitiveCombination, Ellipse, StarshapedPolygon
-from utils import is_ccw, is_collinear, equilateral_triangle, Cone, tic, toc, draw_shapely_polygon
+from ..obstacles import Frame, StarshapedPrimitiveCombination, Ellipse, StarshapedPolygon
+from ..utils import is_ccw, is_collinear, equilateral_triangle, Cone, tic, toc, draw_shapely_polygon
from scipy.spatial import ConvexHull
-import starshaped_hull as sh
+from ..starshaped_hull import starshaped_hull as sh
import matplotlib.pyplot as plt
diff --git a/python/smc/path_generation/starworlds/starshaped_hull/starshaped_hull.py b/python/smc/path_generation/starworlds/starshaped_hull/starshaped_hull.py
index 8fde482f9edaf717ffcb82f170b9bacc158c3714..16e1c4b67e18f35640c58e488bcd66db1dfb0461 100644
--- a/python/smc/path_generation/starworlds/starshaped_hull/starshaped_hull.py
+++ b/python/smc/path_generation/starworlds/starshaped_hull/starshaped_hull.py
@@ -1,7 +1,7 @@
import shapely
import numpy as np
-from obstacles import Polygon
-from utils import is_ccw, is_cw, line, ray, Cone, convex_hull
+from ..obstacles import Polygon
+from ..utils import is_ccw, is_cw, line, ray, Cone, convex_hull
import matplotlib.pyplot as plt
@@ -11,7 +11,7 @@ def admissible_kernel(obstacle, x):
if not tps:
# Interior point
return None
- return Cone(x, x-tps[0], x-tps[1])
+ return Cone(x, x - tps[0], x - tps[1])
# Computes the starshaped hull of a list of obstacles for specified kernel points
@@ -39,7 +39,7 @@ def convex_kernel_starshaped_hull(convex_obstacle, kernel_points):
if not tps:
return shapely.geometry.Polygon([])
- tps = np.unique(tps,axis=0)
+ tps = np.unique(tps, axis=0)
ch_points = np.vstack((tps, kernel_points))
pol = convex_hull(ch_points)
return shapely.geometry.Polygon(pol)
@@ -47,7 +47,7 @@ def convex_kernel_starshaped_hull(convex_obstacle, kernel_points):
# TODO: Improve computational consideration
def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
- kernel_points = kernel_points.reshape((kernel_points.size//2, 2))
+ kernel_points = kernel_points.reshape((kernel_points.size // 2, 2))
if kernel_points.shape[0] > 2:
# NOTE: Assumes kernel points convex
@@ -58,7 +58,7 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
# vertices = np.asarray(polygon_sh.exterior.coords)[:-1, :] # Vertices of polygon
vertices = np.asarray(polygon.exterior.coords)[:-1, :] # Vertices of polygon
star_vertices = [] # Vertices of starshaped hull polygon
- v_bar = kernel_points[0].copy() # Last vertex of starshaped hull polygon
+ v_bar = kernel_points[0].copy() # Last vertex of starshaped hull polygon
e1_idx = 0
e2_idx = 0
k_centroid = np.mean(kernel_points, axis=0)
@@ -78,7 +78,9 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
# Check if no ray r(v,kv) intersects with interior of polygon
if all([ray(v, k, v).disjoint(polygon) for k in kernel_points]):
# Add current vertex
- if kernel_points.shape[0] < 3 or not convex_kernel_subset.contains(shapely.geometry.Point(v)):
+ if kernel_points.shape[0] < 3 or not convex_kernel_subset.contains(
+ shapely.geometry.Point(v)
+ ):
star_vertices += [v]
if star_vertices:
# Intersections of lines l(k,v) and l(e1,e2)
@@ -86,12 +88,16 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
e1_e2 = line(e1, e2)
for k in kernel_points:
- kv_e1e2_intersect = line(k,v).intersection(e1_e2)
+ kv_e1e2_intersect = line(k, v).intersection(e1_e2)
# Adjust to closest intersection to e2
if not kv_e1e2_intersect.is_empty:
adjust_e1 = True
- e1_candidate = np.array([kv_e1e2_intersect.x, kv_e1e2_intersect.y])
- if np.linalg.norm(e2 - e1_candidate) < np.linalg.norm(e2 - star_vertices[e1_idx]):
+ e1_candidate = np.array(
+ [kv_e1e2_intersect.x, kv_e1e2_intersect.y]
+ )
+ if np.linalg.norm(e2 - e1_candidate) < np.linalg.norm(
+ e2 - star_vertices[e1_idx]
+ ):
star_vertices[e1_idx] = e1_candidate
if not adjust_e1:
@@ -102,18 +108,25 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
# If l(k,v) intersects interior of P
if not kv_P_intersect.is_empty:
# Find last intersection of l(k,v) and polygon boundary
- if kv_P_intersect.geom_type == 'LineString':
- intersection_points = [np.array([ip[0], ip[1]]) for ip in kv_P_intersect.coords]
- elif kv_P_intersect.geom_type == 'MultiLineString':
- intersection_points = [np.array([ip[0], ip[1]]) for l in kv_P_intersect.geoms for ip in
- l.coords]
- elif kv_P_intersect.geom_type == 'GeometryCollection':
+ if kv_P_intersect.geom_type == "LineString":
+ intersection_points = [
+ np.array([ip[0], ip[1]]) for ip in kv_P_intersect.coords
+ ]
+ elif kv_P_intersect.geom_type == "MultiLineString":
+ intersection_points = [
+ np.array([ip[0], ip[1]])
+ for l in kv_P_intersect.geoms
+ for ip in l.coords
+ ]
+ elif kv_P_intersect.geom_type == "GeometryCollection":
intersection_points = []
for g in kv_P_intersect.geoms:
- if g.geom_type == 'Point':
+ if g.geom_type == "Point":
intersection_points += [np.array(g.coords[0])]
- if kv_P_intersect.geom_type == 'LineString':
- intersection_points += [np.array([ip[0], ip[1]]) for ip in g.coords]
+ if kv_P_intersect.geom_type == "LineString":
+ intersection_points += [
+ np.array([ip[0], ip[1]]) for ip in g.coords
+ ]
else:
intersection_points = []
@@ -133,30 +146,46 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
for kp in kps:
kpvb_uv_intersect = line(kp, v_bar).intersection(u_v)
if not kpvb_uv_intersect.is_empty:
- u = np.array([kpvb_uv_intersect.x, kpvb_uv_intersect.y])
+ u = np.array(
+ [kpvb_uv_intersect.x, kpvb_uv_intersect.y]
+ )
# Append u to P*
star_vertices += [u]
# Update last augmented edge
- e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ e1_idx, e2_idx = (
+ len(star_vertices) - 1,
+ len(star_vertices) - 2,
+ )
# Swap last vertices if not CCW
- if is_ccw(u, v, vertices[v_idx-1]):
- # if is_ccw(v_bar, v, u):
- # if is_cw(k_centroid, v, u):
- star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ if is_ccw(u, v, vertices[v_idx - 1]):
+ # if is_ccw(v_bar, v, u):
+ # if is_cw(k_centroid, v, u):
+ star_vertices[-2], star_vertices[-1] = (
+ star_vertices[-1],
+ star_vertices[-2],
+ )
e1_idx, e2_idx = e2_idx, e1_idx
adjust_e1 = True
else:
# Check if no ray r(k,k'v) intersect with interior of polygon
- if (not k_included[k_idx]) and (not any([ray(k, kp, v).intersects(polygon) for kp in kps])):
+ if (not k_included[k_idx]) and (
+ not any([ray(k, kp, v).intersects(polygon) for kp in kps])
+ ):
k_included[k_idx] = True
# Append k to P*
star_vertices += [k]
# Update last augmented edge
- e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ e1_idx, e2_idx = (
+ len(star_vertices) - 1,
+ len(star_vertices) - 2,
+ )
# Swap last vertices if not CCW
- if is_ccw(k, v, vertices[v_idx-1]):
- # if is_cw(k_centroid, v, k):
- star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ if is_ccw(k, v, vertices[v_idx - 1]):
+ # if is_cw(k_centroid, v, k):
+ star_vertices[-2], star_vertices[-1] = (
+ star_vertices[-1],
+ star_vertices[-2],
+ )
e1_idx, e2_idx = e2_idx, e1_idx
adjust_e1 = True
# Update v_bar
@@ -164,13 +193,18 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
# Visualize debug information
if debug == 1:
- plt.plot(*k_centroid, 'ko')
- plt.plot(*polygon.exterior.xy, 'k')
- plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
- [plt.plot(*k, 'kx') for k in kernel_points]
- [plt.plot(*line(k,v).xy, 'k--') for k in kernel_points]
+ plt.plot(*k_centroid, "ko")
+ plt.plot(*polygon.exterior.xy, "k")
+ plt.plot(
+ [p[0] for p in star_vertices],
+ [p[1] for p in star_vertices],
+ "g-o",
+ linewidth=2,
+ )
+ [plt.plot(*k, "kx") for k in kernel_points]
+ [plt.plot(*line(k, v).xy, "k--") for k in kernel_points]
if adjust_e1:
- plt.plot(*star_vertices[e1_idx], 'ys')
+ plt.plot(*star_vertices[e1_idx], "ys")
plt.show()
# Check not added kernel points if they should be included
@@ -183,25 +217,33 @@ def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
star_vertices = star_vertices[:j] + [k] + star_vertices[j:]
# Visualize debug information
if debug == 1:
- plt.plot(*k_centroid, 'ko')
- plt.plot(*polygon.exterior.xy, 'k')
- plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
- [plt.plot(*ki, 'kx') for ki in kernel_points]
- plt.plot(*line(k, v).xy, 'r--*')
- plt.plot(*line(k, vp).xy, 'r--*')
- plt.plot(*k, 'go')
+ plt.plot(*k_centroid, "ko")
+ plt.plot(*polygon.exterior.xy, "k")
+ plt.plot(
+ [p[0] for p in star_vertices],
+ [p[1] for p in star_vertices],
+ "g-o",
+ linewidth=2,
+ )
+ [plt.plot(*ki, "kx") for ki in kernel_points]
+ plt.plot(*line(k, v).xy, "r--*")
+ plt.plot(*line(k, vp).xy, "r--*")
+ plt.plot(*k, "go")
plt.show()
# print("Final kernel check: {:.1f}".format(toc(t0)))
if debug:
ax = plt.gca()
- ax.plot(*polygon.exterior.xy, 'k')
- ax.plot([p[0] for p in star_vertices] + [star_vertices[0][0]],
- [p[1] for p in star_vertices] + [star_vertices[0][1]], 'g-o', linewidth=2)
+ ax.plot(*polygon.exterior.xy, "k")
+ ax.plot(
+ [p[0] for p in star_vertices] + [star_vertices[0][0]],
+ [p[1] for p in star_vertices] + [star_vertices[0][1]],
+ "g-o",
+ linewidth=2,
+ )
# [ax.plot(star_vertices[i][0], star_vertices[i][1], 'r*') for i in augmented_vertex_idcs]
- [ax.plot(*zip(k, sv), 'y--') for sv in star_vertices for k in kernel_points]
- ax.plot(*k_centroid, 'bs')
+ [ax.plot(*zip(k, sv), "y--") for sv in star_vertices for k in kernel_points]
+ ax.plot(*k_centroid, "bs")
plt.show()
return shapely.geometry.Polygon(star_vertices)
-
diff --git a/python/smc/path_generation/starworlds/utils/cg.py b/python/smc/path_generation/starworlds/utils/cg.py
index bb7525d45bbaef9404d59fb15fa2c32d9e666f12..d9ae557ca4cb8c67c9ba265b33e1d033a2c29305 100644
--- a/python/smc/path_generation/starworlds/utils/cg.py
+++ b/python/smc/path_generation/starworlds/utils/cg.py
@@ -3,7 +3,7 @@ import shapely.geometry
import shapely.ops
import matplotlib.pyplot as plt
from typing import List, Tuple
-from utils import draw_shapely_polygon
+from ..utils import draw_shapely_polygon
DEFAULT_RAY_INFINITY_LENGTH = 100000.
COLLINEAR_THRESHOLD = 1e-10
diff --git a/python/smc/robots/__init__.py b/python/smc/robots/__init__.py
index dbe9d2fcebbbcb4240232eebf4bfda57a94166bd..a46ef63cec4e015eb2c7a7cba321df46008e2d2e 100644
--- a/python/smc/robots/__init__.py
+++ b/python/smc/robots/__init__.py
@@ -7,4 +7,4 @@ from .interfaces.single_arm_interface import SingleArmInterface
from .interfaces.dual_arm_interface import DualArmInterface
from .interfaces.force_torque_sensor_interface import ForceTorqueOnSingleArmWrist
-import utils
+from .utils import *
diff --git a/python/smc/robots/implementations/__init__.py b/python/smc/robots/implementations/__init__.py
index 78ca8f8c7c58e18b09f122a8bc5a6ed952642813..6a9cc722e3707207c452e8f44e8ef2e990e2e8be 100644
--- a/python/smc/robots/implementations/__init__.py
+++ b/python/smc/robots/implementations/__init__.py
@@ -1,4 +1,9 @@
+# from .heron import RobotManagerHeron, SimulatedHeronRobotManager
from .heron import *
from .mir import *
from .mobile_yumi import *
-from .ur5e import *
+from .ur5e import RobotManagerUR5e, SimulatedUR5eRobotManager
+from importlib.util import find_spec
+
+if find_spec("rtde_control"):
+ from .ur5e import RealUR5eRobotManager
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index dbbcb888cab1674ca19dee53148780db640752e2..d50ce5de8b0e343d16d427c5d9a2fd070ecf4f4d 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -9,12 +9,15 @@ from smc.robots.abstract_simulated_robot import AbstractSimulatedRobotManager
import numpy as np
import pinocchio as pin
from importlib.resources import files
+from importlib.util import find_spec
import time
import argparse
from os import path
-from rtde_control import RTDEControlInterface
-from rtde_receive import RTDEReceiveInterface
-from rtde_io import RTDEIOInterface
+
+if find_spec("rtde_control"):
+ from rtde_control import RTDEControlInterface
+ from rtde_receive import RTDEReceiveInterface
+ from rtde_io import RTDEIOInterface
# NOTE: this one assumes a jaw gripper
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index 8c2e9044a0c1ce8cd67dc79b4373fb70d1c514bf..e93a7a1ab2c2f634c11cbc9c37da1f12ee78693f 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -1,4 +1,5 @@
import numpy as np
+
import pinocchio as pin
from smc.robots.robotmanager_abstract import AbstractRobotManager
diff --git a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
index 45b02cc8a1eb6fac460e9c67b4575c52facd7648..ca2e1fe364e423735eca7434a2d83f800e918ab5 100644
--- a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
+++ b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
@@ -106,9 +106,12 @@ class MeshcatVisualizer(PMV):
self.applyConfiguration(point_name, point)
self.n_points += 1
- def addPath(self, name, path: list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]):
+ def addPath(
+ self, name, path: list[pin.SE3] | np.ndarray, radius=5e-3, color=[1, 0, 0, 0.8]
+ ):
# who cares about the name
name = "path"
+ self.n_path_points = len(path)
if type(path) == np.ndarray:
# complete the quartenion
path = np.hstack((path, np.zeros((len(path), 3))))