├── .gitignore ├── .pylintrc ├── .python-version ├── LICENSE.txt ├── README.md ├── cppflow ├── __init__.py ├── collision_detection.py ├── config.py ├── data_type_utils.py ├── data_types.py ├── evaluation_utils.py ├── lm_hyper_parameters.py ├── optimization.py ├── optimization_utils.py ├── paths │ ├── 1cube.csv │ ├── 1cube_mini.csv │ ├── 2cubes.csv │ ├── README.md │ ├── circle.csv │ ├── flappy_bird.csv │ ├── hello.csv │ ├── hello_mini.csv │ ├── paths_torm │ │ ├── circle │ │ ├── hello │ │ ├── rot_yz │ │ ├── s │ │ ├── s_two │ │ └── square │ ├── rot_yz.csv │ ├── rot_yz2.csv │ ├── s.csv │ ├── s_truncated.csv │ ├── square.csv │ └── update_path_format.py ├── planners.py ├── problems │ ├── README.md │ ├── fetch__circle.yaml │ ├── fetch__hello.yaml │ ├── fetch__rot_yz.yaml │ ├── fetch__rot_yz2.yaml │ ├── fetch__s.yaml │ ├── fetch__square.yaml │ ├── fetch_arm__circle.yaml │ ├── fetch_arm__hello.yaml │ ├── fetch_arm__hello_mini.yaml │ ├── fetch_arm__rot_yz.yaml │ ├── fetch_arm__rot_yz2.yaml │ ├── fetch_arm__s.yaml │ ├── fetch_arm__square.yaml │ ├── panda__1cube.yaml │ ├── panda__1cube_mini.yaml │ ├── panda__2cubes.yaml │ ├── panda__flappy_bird.yaml │ └── panda__square.yaml ├── ros2 │ ├── __init__.py │ ├── cppflow │ ├── resources │ │ ├── CppFlowEnvironmentConfig_request.bin │ │ ├── CppFlowQuery_request.bin │ │ └── __init__.py │ ├── ros2_publisher.py │ ├── ros2_subscriber.py │ └── ros2_utils.py ├── search.py ├── utils.py └── visualization.py ├── lint ├── main.py ├── package.xml ├── pyproject.toml ├── run_tests ├── scripts ├── benchmark.py ├── benchmarking_output │ └── .gitkeep ├── create_path.py └── evaluate.py ├── setup.cfg ├── setup.py ├── tests ├── collision_checking_test.py ├── evaluation_utils_test.py ├── fetch__s__truncated.yaml ├── fetch_arm__s__truncated.yaml ├── optimization_test.py ├── optimization_utils_test.py ├── planners_test.py ├── problem_test.py ├── search_test.py ├── test_data │ └── test_problem.yaml └── utils_test.py └── uv.lock /.gitignore: -------------------------------------------------------------------------------- 1 | notes 2 | __pycache__/ 3 | venv/ 4 | venv-test/ 5 | cppflow.egg-info/ 6 | pt_tensors/*.pt 7 | .ipynb_checkpoints/ 8 | *.pkl 9 | dist/ 10 | build/ 11 | scripts/stampede_ee_pose_testing.py 12 | lightning_logs/ 13 | scripts/benchmarking_output/*.csv 14 | scripts/benchmarking_output/*.md 15 | *.pdf 16 | *.png 17 | .vscode/ 18 | PLANNER_RESULTS.md 19 | scripts/test.py 20 | notebooks/*.pt 21 | notebooks/images/*/ 22 | notebooks/reports/*/ 23 | PLANNER_RESULTS*.md 24 | notebooks/*.csv 25 | .ruff_cache/ 26 | .venv/ 27 | .pytest_cache/ 28 | -------------------------------------------------------------------------------- /.pylintrc: -------------------------------------------------------------------------------- 1 | [MESSAGES CONTROL] 2 | 3 | disable=missing-module-docstring,invalid-name,missing-function-docstring,missing-class-docstring,no-member,fixme 4 | 5 | [FORMAT] 6 | 7 | max-line-length=120 -------------------------------------------------------------------------------- /.python-version: -------------------------------------------------------------------------------- 1 | 3.10 2 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2024 Jeremy Morgan 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining 4 | a copy of this software and associated documentation files (the 5 | "Software"), to deal in the Software without restriction, including 6 | without limitation the rights to use, copy, modify, merge, publish, 7 | distribute, sublicense, and/or sell copies of the Software, and to 8 | permit persons to whom the Software is furnished to do so, subject to 9 | the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be 12 | included in all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 15 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 16 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 17 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 18 | LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 19 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 20 | WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CppFlow 2 | 3 | Cartesian path planning with IKFlow. Open source implementation to the paper ["CppFlow: Generative Inverse Kinematics for Efficient and Robust Cartesian Path Planning"](https://arxiv.org/abs/2309.09102) 4 | 5 | [![arxiv.org](https://img.shields.io/badge/cs.RO-%09arXiv%3A2309.09102-red)](https://arxiv.org/abs/2309.09102) 6 | 7 | 8 | Note: This project uses the `w,x,y,z` format for quaternions. 9 | 10 | ## Installation 11 | 12 | ``` 13 | git clone https://github.com/jstmn/cppflow.git && cd cppflow 14 | uv sync 15 | uv pip install -e . 16 | ``` 17 | 18 | ## Getting started 19 | 20 | Generate a plan for a single problem 21 | ``` bash 22 | 23 | # Problems: 24 | # - fetch__circle 25 | # - fetch__hello 26 | # - fetch__rot_yz 27 | # - fetch__s 28 | # - fetch__square 29 | # - fetch_arm__circle 30 | # - fetch_arm__hello 31 | # - fetch_arm__rot_yz 32 | # - fetch_arm__s 33 | # - fetch_arm__square 34 | # - panda__flappy_bird 35 | # - panda__2cubes 36 | # - panda__1cube 37 | 38 | # you can replace 'fetch__hello' with any of the problems listed above 39 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__hello --visualize 40 | ``` 41 | 42 | Recreate the results from the paper: 43 | ``` bash 44 | git checkout 2b6ad3097ad06af17e8d7eacdff78bbc98a1c3be 45 | uv run python scripts/benchmark.py --planner_name=CppFlowPlanner 46 | ``` 47 | 48 | 49 | 50 | ## Citation 51 | 52 | ``` 53 | @INPROCEEDINGS{10611724, 54 | author={Morgan, Jeremy and Millard, David and Sukhatme, Gaurav S.}, 55 | booktitle={2024 IEEE International Conference on Robotics and Automation (ICRA)}, 56 | title={CppFlow: Generative Inverse Kinematics for Efficient and Robust Cartesian Path Planning}, 57 | year={2024}, 58 | volume={}, 59 | number={}, 60 | pages={12279-12785}, 61 | keywords={Adaptation models;Generative AI;Graphics processing units;Kinematics;Programming;Trajectory;Planning}, 62 | doi={10.1109/ICRA57147.2024.10611724} 63 | } 64 | ``` -------------------------------------------------------------------------------- /cppflow/__init__.py: -------------------------------------------------------------------------------- 1 | from . import collision_detection 2 | from . import evaluation_utils 3 | from . import lm_hyper_parameters 4 | from . import optimization_utils 5 | from . import optimization 6 | from . import planners 7 | from . import search 8 | from . import utils 9 | from . import visualization 10 | from . import data_types 11 | from . import data_type_utils 12 | 13 | __all__ = [ 14 | "collision_detection", 15 | "evaluation_utils", 16 | "lm_hyper_parameters", 17 | "optimization_utils", 18 | "optimization", 19 | "planners", 20 | "search", 21 | "utils", 22 | "visualization", 23 | "data_types", 24 | "data_type_utils", 25 | ] 26 | -------------------------------------------------------------------------------- /cppflow/collision_detection.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | 3 | import torch 4 | 5 | from cppflow.data_types import Problem 6 | from cppflow.config import DEVICE 7 | 8 | 9 | def get_only_non_colliding_qpaths( 10 | qpaths: List[torch.Tensor], self_colliding: torch.Tensor, env_colliding: torch.Tensor 11 | ): 12 | """Return only qpaths which are non colliding (robot-robot and robot-env). 13 | 14 | Args: 15 | qpaths (List[torch.Tensor]): List of [ntimesteps x ndof] tensors of configs 16 | self_colliding (torch.Tensor): [k x ntimesteps] tensor of bools indicating if the config results in a self 17 | collision at each timestep 18 | env_colliding (torch.Tensor): [k x ntimesteps] tensor of bools indicating if the config is in collision with the 19 | environment at each timestep 20 | """ 21 | assert len(qpaths) == self_colliding.shape[0] == env_colliding.shape[0] 22 | colliding_idxs = torch.logical_or(self_colliding, env_colliding) 23 | to_keep = torch.sum(colliding_idxs, dim=1) == 0 24 | return [qpaths[i] for i in to_keep.nonzero()[:, 0]] 25 | 26 | 27 | def qpaths_batched_env_collisions(problem: Problem, q: torch.Tensor) -> torch.Tensor: 28 | """Returns boolean tensor where 1 indicates the configuration is in collision with itself 29 | 30 | Args: 31 | q (torch.Tensor): [k x ntimesteps x n_dofs] 32 | 33 | Returns: 34 | torch.Tensor: [k x ntimesteps] 35 | """ 36 | k, n, ndof = q.shape 37 | colliding = torch.zeros((k, n), dtype=torch.bool) 38 | q_2d = q.reshape((k * n, ndof)) 39 | for obstacle_cuboid, obstacle_Tcuboid in zip(problem.obstacles_cuboids, problem.obstacles_Tcuboids): 40 | dists = problem.robot.env_collision_distances(q_2d, obstacle_cuboid, obstacle_Tcuboid) 41 | min_dists, _ = torch.min(dists, dim=1) 42 | assert min_dists.numel() == n * k 43 | colliding = torch.logical_or(colliding, (min_dists < 0).reshape((k, n))) 44 | # artificially set the collision threshold to get slightly in collision dp_search paths 45 | # import warnings 46 | # warnings.warn(f"FYI: setting {margin_m} m margin for environment collisions. REMOVE THIS AFTER DEBUGGING.") 47 | # margin_m = -0.01 48 | # colliding = torch.logical_or(colliding, (min_dists < margin_m).reshape((k, n))) 49 | return colliding 50 | 51 | 52 | def qpaths_batched_self_collisions(problem: Problem, q: torch.Tensor) -> torch.Tensor: 53 | """Returns boolean tensor where 1 indicates the configuration is in collision with the environment 54 | 55 | Args: 56 | q (torch.Tensor): [k x ntimesteps x n_dofs] 57 | 58 | Returns: 59 | torch.Tensor: [k x ntimesteps] 60 | """ 61 | assert str(q.device) == DEVICE, f"q.device != device ({q.device} == {DEVICE})" 62 | k, n, ndof = q.shape 63 | q_input = q.reshape((k * n, ndof)) 64 | 65 | dists = problem.robot.self_collision_distances(q_input) 66 | min_dists, _ = torch.min(dists, dim=1) 67 | colliding = min_dists < 0 68 | colliding = colliding.reshape((k, n)) 69 | return colliding 70 | 71 | 72 | def self_colliding_configs_capsule(problem: Problem, qpath: torch.Tensor) -> torch.Tensor: 73 | dists = problem.robot.self_collision_distances(qpath) 74 | return torch.min(dists, dim=1)[0] < 0 75 | 76 | 77 | def env_colliding_configs_capsule(problem: Problem, qpath: torch.Tensor) -> torch.Tensor: 78 | """Returns a boolean tensor of shape (n_timesteps,) where each value is True if the corresponding config in 79 | qpath is colliding with the environment and False otherwise. 80 | """ 81 | colliding = torch.zeros(problem.n_timesteps, dtype=torch.bool) 82 | for obstacle_cuboid, obstacle_Tcuboid in zip(problem.obstacles_cuboids, problem.obstacles_Tcuboids): 83 | dists = problem.robot.env_collision_distances(qpath, obstacle_cuboid, obstacle_Tcuboid) 84 | min_dists = torch.min(dists, dim=1)[0] 85 | colliding = torch.logical_or(colliding, min_dists < 0) 86 | return colliding 87 | 88 | 89 | def env_colliding_configs_klampt(problem: Problem, qpath: torch.Tensor) -> torch.Tensor: 90 | colliding = torch.zeros(problem.n_timesteps, dtype=torch.bool) 91 | for i, x in enumerate(qpath.cpu().numpy()): 92 | for j, _ in enumerate(problem.obstacles_klampt): 93 | try: 94 | if colliding[i]: 95 | continue 96 | except IndexError as e: 97 | raise RuntimeError( 98 | f"env_colliding_configs_klampt() | index error for 'colliding[i]'\nproblem: {problem}, qpath:" 99 | f" {qpath.shape}, i: {i}, x: {x}" 100 | ) from e 101 | 102 | # Note: config_collides_with_env() accepts an int which refers to the index of the RigidBodyObject saved in 103 | # the WorldModel which is saved in robot. Obstacles are added to the WorldModel() in problem.py when calling 104 | # create.box(world=robot.world_model). 105 | colliding[i] = problem.robot.config_collides_with_env(x, j) 106 | return colliding 107 | 108 | 109 | def self_colliding_configs_klampt(problem: Problem, qpath: torch.Tensor) -> torch.Tensor: 110 | colliding = torch.zeros(problem.n_timesteps, dtype=torch.bool) 111 | for i, x in enumerate(qpath.cpu().numpy()): 112 | try: 113 | colliding[i] = problem.robot.config_self_collides(x) 114 | except IndexError as e: 115 | raise RuntimeError( 116 | "self_colliding_configs_klampt() | index error for 'colliding[i] =" 117 | f" problem.robot.config_self_collides(x)'\nproblem: {problem}, qpath: {qpath.shape}, i: {i}, x: {x}" 118 | ) from e 119 | 120 | return colliding 121 | 122 | 123 | def env_colliding_links_klampt(problem: Problem, q: torch.Tensor) -> List[str]: 124 | """Returns a list of links that are colliding with the environment at the given configuration.""" 125 | links = [] 126 | for j in range(len(problem.obstacles_klampt)): 127 | collisions = problem.robot.config_collides_with_env(q, j, return_detailed=True) 128 | # RobotModelLink, RigidObjectModel 129 | for robot_link, _ in collisions: # robot_link, rigid_object 130 | links.append(robot_link.getName()) 131 | return list(set(links)) 132 | 133 | 134 | def env_colliding_links_capsule(problem: Problem, q: torch.Tensor) -> List[str]: 135 | """Returns a list of links that are colliding with the environment at the given configuration.""" 136 | links = [] 137 | ordered_links = list(problem.robot._collision_capsules_by_link.keys()) 138 | for obstacle_cuboid, obstacle_Tcuboid in zip(problem.obstacles_cuboids, problem.obstacles_Tcuboids): 139 | dists = problem.robot.env_collision_distances(q.unsqueeze(0), obstacle_cuboid, obstacle_Tcuboid) 140 | for i in range(dists.shape[1]): 141 | if dists[0, i] < 0: 142 | links.append(ordered_links[i]) 143 | return list(set(links)) 144 | -------------------------------------------------------------------------------- /cppflow/config.py: -------------------------------------------------------------------------------- 1 | import warnings 2 | 3 | import torch 4 | from jrl.config import DEVICE 5 | 6 | print("cppflow/config.py | device:", DEVICE) 7 | 8 | DEFAULT_TORCH_DTYPE = torch.float32 9 | torch.set_default_dtype(DEFAULT_TORCH_DTYPE) 10 | torch.set_default_device(DEVICE) 11 | 12 | VERBOSITY = 2 13 | 14 | # SUCCESS_THRESHOLD_initial_q_norm_dist = 0.01 15 | SUCCESS_THRESHOLD_initial_q_norm_dist = 0.2 16 | 17 | DEFAULT_RERUN_MJAC_THRESHOLD_DEG = 13.0 18 | DEFAULT_RERUN_MJAC_THRESHOLD_CM = 3.42 19 | OPTIMIZATION_CONVERGENCE_THRESHOLD = 0.005 20 | 21 | 22 | # LM optimization 23 | SELF_COLLISIONS_IGNORED = False 24 | ENV_COLLISIONS_IGNORED = False 25 | DEBUG_MODE_ENABLED = False 26 | 27 | if SELF_COLLISIONS_IGNORED: 28 | warnings.warn("robot-robot are collisions will be ignored during LM optimization") 29 | if ENV_COLLISIONS_IGNORED: 30 | warnings.warn("environment-robot collisions will be ignored during LM optimization") 31 | -------------------------------------------------------------------------------- /cppflow/data_type_utils.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, List, Dict 2 | import os 3 | import csv 4 | 5 | import yaml 6 | from jrl.robot import Robot 7 | from jrl.robots import get_robot 8 | from jrl.math_utils import rpy_tuple_to_rotation_matrix 9 | import numpy as np 10 | from klampt.math import so3 11 | from klampt.model import create 12 | import torch 13 | 14 | 15 | from cppflow.config import ENV_COLLISIONS_IGNORED, SELF_COLLISIONS_IGNORED 16 | from cppflow.evaluation_utils import positional_errors, rotational_errors 17 | from cppflow.collision_detection import self_colliding_configs_klampt, env_colliding_configs_klampt 18 | from cppflow.data_types import Problem, Plan, Constraints 19 | from cppflow import config 20 | from cppflow.utils import get_filepath, to_torch 21 | 22 | np.set_printoptions(suppress=True) 23 | 24 | ALL_PROBLEM_FILENAMES = [ 25 | "fetch_arm__hello", 26 | "fetch_arm__circle", 27 | # "fetch_arm__rot_yz", 28 | "fetch_arm__rot_yz2", 29 | "fetch_arm__s", 30 | "fetch_arm__square", 31 | "fetch__circle", 32 | "fetch__hello", 33 | # "fetch__rot_yz", 34 | "fetch__rot_yz2", 35 | "fetch__s", 36 | "fetch__square", 37 | "panda__1cube", 38 | "panda__2cubes", 39 | "panda__flappy_bird", 40 | ] 41 | 42 | ALL_OBS_PROBLEM_FILENAMES = [ 43 | "fetch_arm__circle", 44 | "fetch_arm__s", 45 | "fetch_arm__square", 46 | "fetch__circle", 47 | "fetch__s", 48 | "fetch__square", 49 | "panda__1cube", 50 | "panda__2cubes", 51 | "panda__flappy_bird", 52 | ] 53 | 54 | 55 | def offset_target_path( 56 | robot: Robot, 57 | target_path: torch.Tensor, 58 | path_offset_frame: str, 59 | xyz_offset: List[float], 60 | R_offset: List[List[float]], 61 | ) -> torch.Tensor: 62 | """Offset the target path by a given xyz and rotation offset.""" 63 | path = target_path.copy() # copy to avoid any kind of weirdness 64 | 65 | if path_offset_frame == "world": 66 | world_T_path_offset = np.zeros(3) 67 | else: 68 | world_T_path_offset = robot.forward_kinematics_klampt( 69 | np.zeros(robot.ndof)[None, :], link_name=path_offset_frame 70 | )[0] 71 | # The path_offset reference frame can't be rotated w.r.t. the world frame, for now. This isn't a permenant 72 | # requirement, I just haven't implemented it yet 73 | np.testing.assert_allclose(world_T_path_offset[3:], np.array([1, 0, 0, 0]), atol=1e-8) 74 | 75 | for i in range(3): 76 | path[:, i] += xyz_offset[i] + world_T_path_offset[i] 77 | 78 | R_offset_klampt = so3.from_ndarray(np.array(R_offset)) 79 | for i in range(path.shape[0]): 80 | R_i_klampt = so3.from_quaternion(path[i, 3:7]) 81 | R_updated = so3.mul(R_i_klampt, R_offset_klampt) 82 | q_updated = np.array(so3.quaternion(R_updated)) 83 | path[i, 3:7] = q_updated 84 | return to_torch(path) 85 | 86 | 87 | def get_obstacles(robot: Robot, problem_dict: Dict, device=config.DEVICE): 88 | obstacles_unparsed = problem_dict["obstacles"] if "obstacles" in problem_dict else [] 89 | obstacles = obstacles_unparsed 90 | obstacles_Tcuboids = [] 91 | obstacles_cuboids = [] 92 | if len(obstacles_unparsed) > 0: 93 | # assert "obstacle_xyz_offset" in problem_dict, f"'obstacle_xyz_offset' not found for {problem_filename}" 94 | obstacles_parsed = [] 95 | for obs in obstacles: 96 | parsed = {} 97 | for d in obs: 98 | for k, v in d.items(): 99 | parsed[k] = v 100 | # Shift obstacles by 'obstacle_xyz_offset' 101 | parsed["x"] += problem_dict["obstacle_xyz_offset"][0] 102 | parsed["y"] += problem_dict["obstacle_xyz_offset"][1] 103 | parsed["z"] += problem_dict["obstacle_xyz_offset"][2] 104 | # 105 | obstacles_parsed.append(parsed) 106 | obstacles = obstacles_parsed 107 | for obs in obstacles: 108 | assert abs(obs["roll"]) < 1e-8 and abs(obs["pitch"]) < 1e-8 and abs(obs["yaw"]) < 1e-8 109 | cuboid = torch.tensor( 110 | [ 111 | -obs["size_x"] / 2, 112 | -obs["size_y"] / 2, 113 | -obs["size_z"] / 2, 114 | obs["size_x"] / 2, 115 | obs["size_y"] / 2, 116 | obs["size_z"] / 2, 117 | ], 118 | device=device, 119 | ) 120 | Tcuboid = torch.zeros((4, 4), device=device) 121 | Tcuboid[0, 3] = obs["x"] 122 | Tcuboid[1, 3] = obs["y"] 123 | Tcuboid[2, 3] = obs["z"] 124 | Tcuboid[:3, :3] = rpy_tuple_to_rotation_matrix((obs["roll"], obs["pitch"], obs["yaw"])) 125 | 126 | obstacles_Tcuboids.append(Tcuboid) 127 | obstacles_cuboids.append(cuboid) 128 | 129 | R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 130 | obstacles_klampt = [ 131 | create.box( 132 | width=obs["size_x"], 133 | height=obs["size_z"], 134 | depth=obs["size_y"], 135 | center=(0, 0, 0), 136 | t=(obs["x"], obs["y"], obs["z"]), 137 | R=R, 138 | mass=1, 139 | world=robot._klampt_world_model, # passing robot._klampt_world_model adds the box as a RigidObjectModel to robot._klampt_world_model 140 | ) 141 | for obs in obstacles 142 | ] 143 | robot.make_new_collision_checker() 144 | 145 | return obstacles, obstacles_Tcuboids, obstacles_cuboids, obstacles_klampt 146 | 147 | 148 | def problem_from_filename( 149 | constraints: Constraints, 150 | problem_filename: str, 151 | filepath_override: Optional[str] = None, 152 | robot: Optional[Robot] = None, 153 | ) -> Problem: 154 | """Parse a yaml file and create a Problem object. 155 | 156 | A note: about 'obstacle_xyz_offset'. Some of the torm problems set the fetch torso_lift_link to 0.2. To account for 157 | this, the paths are optionally shifted so that the are in the correct location when the torso_lift_link is at 0, 158 | which it is for the Fetch.Arm problems in this directory. 159 | 160 | Args: 161 | problem_filename (str): _description_ 162 | filepath_override (Optional[str], optional): Optionally provide the filepath directly. Used for testing. 163 | Defaults to None. 164 | """ 165 | 166 | # Get the filepath to the problem definition 167 | if filepath_override is None: 168 | assert ( 169 | "yaml" not in problem_filename 170 | ), f"problem_filename should not include the .yaml file extension. '{problem_filename}'" 171 | filepath = get_filepath(os.path.join("problems/", problem_filename + ".yaml")) 172 | else: 173 | filepath = filepath_override 174 | 175 | # Parse the problem definition 176 | with open(filepath, "r") as f: 177 | problem_dict = yaml.load(f, Loader=yaml.FullLoader) 178 | 179 | if robot is None: 180 | robot = get_robot(problem_dict["robot"]) 181 | else: 182 | assert "obstacles" not in problem_dict, f"Error - obstacles found for {problem_filename} but robot is provided" 183 | 184 | obstacles, obstacles_Tcuboids, obstacles_cuboids, obstacles_klampt = get_obstacles(robot, problem_dict) 185 | assert robot._klampt_world_model.numRigidObjects() == len( 186 | obstacles 187 | ), f"Error - {robot._klampt_world_model.numRigidObjects()} != {len(obstacles)}" 188 | 189 | # Load the end effector path 190 | path_name = problem_dict["path_name"] 191 | with open(get_filepath(os.path.join("paths/", path_name + ".csv")), "r") as csvfile: 192 | csvreader = csv.reader(csvfile, delimiter=",") 193 | rows = [[float(x) for x in row] for i, row in enumerate(csvreader) if i > 0] 194 | 195 | # Offset the end effector path according to 'path_xyz_offset', 'path_R_offset'. These are specified in the 196 | # 'path_offset_frame' frame 197 | # NOTE: 'path_R_offset' applies a rotation for each individual pose in the path, it does not rotate the path's 198 | # reference frame. 199 | original_path = np.array(rows)[:, 1:] 200 | target_path = offset_target_path( 201 | robot, 202 | original_path, 203 | problem_dict["path_offset_frame"], 204 | problem_dict["path_xyz_offset"], 205 | problem_dict["path_R_offset"], 206 | ) 207 | name = path_name 208 | return Problem( 209 | constraints, 210 | target_path, 211 | None, 212 | robot, 213 | name, 214 | problem_filename, 215 | obstacles, 216 | obstacles_Tcuboids, 217 | obstacles_cuboids, 218 | obstacles_klampt, 219 | ) 220 | 221 | 222 | def get_problem_dict(problem_names: List[str]) -> Dict[str, Problem]: 223 | robot_map = {pr: None for pr in problem_names} 224 | if "fetch__hello" in problem_names and "fetch__rot_yz2" in problem_names: 225 | fetch = get_robot("fetch") 226 | robot_map["fetch__hello"] = fetch 227 | robot_map["fetch__rot_yz2"] = fetch 228 | print("FYI: using one Fetch robot for 'fetch__hello' and 'fetch__rot_yz2'") 229 | 230 | if "fetch_arm__hello" in problem_names and "fetch_arm__rot_yz2" in problem_names: 231 | fetch_arm = get_robot("fetch_arm") 232 | robot_map["fetch_arm__hello"] = fetch_arm 233 | robot_map["fetch_arm__rot_yz2"] = fetch_arm 234 | print("FYI: using one FetchArm robot for 'fetch_arm__hello' and 'fetch_arm__rot_yz2'") 235 | 236 | return {pname: problem_from_filename(pname, robot=robot_map[pname]) for pname in problem_names} 237 | 238 | 239 | def get_all_problems() -> List[Problem]: 240 | problem_dict = get_problem_dict(ALL_PROBLEM_FILENAMES) 241 | return [problem_dict[pname] for pname in ALL_PROBLEM_FILENAMES] 242 | 243 | 244 | def plan_from_qpath(qpath: torch.Tensor, problem: Problem) -> Plan: 245 | """Converts a qpath to a plan. 246 | 247 | Note: mean runtime is 0.08366363048553467. Don't call this when timed 248 | """ 249 | assert isinstance(qpath, torch.Tensor), f"qpath must be a torch.Tensor, got {type(qpath)}" 250 | traced_path = problem.robot.forward_kinematics(qpath, out_device=qpath.device) 251 | qpath_revolute, qpath_prismatic = problem.robot.split_configs_to_revolute_and_prismatic(qpath) 252 | 253 | # Use klampts collision checker here instead of jrl's capsule-capsule checking. klampt uses the collision 254 | # geometry of the robot which is a tighter bound. 255 | self_colliding = self_colliding_configs_klampt(problem, qpath) 256 | if SELF_COLLISIONS_IGNORED: 257 | self_colliding = torch.zeros_like(self_colliding) 258 | 259 | env_colliding = env_colliding_configs_klampt(problem, qpath) 260 | if ENV_COLLISIONS_IGNORED: 261 | env_colliding = torch.zeros_like(env_colliding) 262 | 263 | return Plan( 264 | q_path=qpath, 265 | q_path_revolute=qpath_revolute, 266 | q_path_prismatic=qpath_prismatic, 267 | pose_path=traced_path, 268 | target_path=problem.target_path, 269 | robot_joint_limits=problem.robot.actuated_joints_limits, 270 | self_colliding_per_ts=self_colliding, 271 | env_colliding_per_ts=env_colliding, 272 | positional_errors=positional_errors(traced_path, problem.target_path), 273 | rotational_errors=rotational_errors(traced_path, problem.target_path), 274 | provided_initial_configuration=problem.initial_configuration, 275 | constraints=problem.constraints, 276 | ) 277 | -------------------------------------------------------------------------------- /cppflow/evaluation_utils.py: -------------------------------------------------------------------------------- 1 | from typing import List, Tuple 2 | 3 | from jrl.robot import Robot 4 | from jrl.math_utils import geodesic_distance_between_quaternions 5 | 6 | import numpy as np 7 | import torch 8 | 9 | from cppflow.config import DEVICE 10 | 11 | # ================================= 12 | # == Trajectory validation == 13 | # 14 | 15 | 16 | def joint_limits_exceeded(robot_joint_limits: List[Tuple[float, float]], qs: np.ndarray) -> Tuple[bool, List[float]]: 17 | """Return the percent of configurations for each joint that are violating the given joint limits.""" 18 | assert len(robot_joint_limits) == qs.shape[1] 19 | n = qs.shape[0] 20 | violation_pcts = [] 21 | for i, (l, u) in enumerate(robot_joint_limits): 22 | assert l < u 23 | qs_i = qs[:, i] 24 | n_violating = (qs_i < l).sum() + (u < qs_i).sum() 25 | violation_pcts.append(100 * n_violating / n) 26 | return any([vp > 0 for vp in violation_pcts]), violation_pcts 27 | 28 | 29 | def errors_are_below_threshold( 30 | max_allowed_position_error_cm: float, 31 | max_allowed_rotation_error_deg: float, 32 | max_allowed_mjac_deg: float, 33 | max_allowed_mjac_cm: float, 34 | error_t_cm: torch.Tensor, 35 | error_R_deg: torch.Tensor, 36 | qdeltas_revolute_deg: torch.Tensor, 37 | qdeltas_prismatic_cm: torch.Tensor, 38 | verbosity: int = 0, 39 | ) -> bool: 40 | """Check whether the given errors are below the set success thresholds.""" 41 | pose_pos_valid = (error_t_cm.max() < max_allowed_position_error_cm).item() 42 | pose_rot_valid = (error_R_deg.max() < max_allowed_rotation_error_deg).item() 43 | if verbosity > 0 and not pose_pos_valid: 44 | print( 45 | "errors_are_below_threshold() | pose-position is invalid (error_t_cm.max() <" 46 | f" max_allowed_position_error_cm): {error_t_cm.max()} <" 47 | f" {max_allowed_position_error_cm}" 48 | ) 49 | if verbosity > 0 and not pose_rot_valid: 50 | print( 51 | "errors_are_below_threshold() | pose-rotation is invalid (error_R_deg.max() <" 52 | f" max_allowed_rotation_error_deg): {error_R_deg.max()} < {max_allowed_rotation_error_deg}" 53 | ) 54 | 55 | mjac_rev_valid = qdeltas_revolute_deg.abs().max() < max_allowed_mjac_deg 56 | mjac_pris_valid = ( 57 | qdeltas_prismatic_cm.abs().max() < max_allowed_mjac_cm if qdeltas_prismatic_cm.numel() > 0 else True 58 | ) 59 | if verbosity > 0: 60 | if not mjac_rev_valid: 61 | print( 62 | f"errors_are_below_threshold() | mjac_rev is invalid: {qdeltas_revolute_deg.abs().max():.3f} >" 63 | f" {max_allowed_mjac_deg:.3f}" 64 | ) 65 | if not mjac_pris_valid: 66 | print( 67 | f"errors_are_below_threshold() | mjac_pris is invalid: {qdeltas_prismatic_cm.abs().max():.3f} >" 68 | f" {max_allowed_mjac_cm:.3f}" 69 | ) 70 | return pose_pos_valid and pose_rot_valid and mjac_rev_valid and mjac_pris_valid, ( 71 | pose_pos_valid, 72 | pose_rot_valid, 73 | mjac_rev_valid, 74 | mjac_pris_valid, 75 | ) 76 | 77 | 78 | # =============================== 79 | # == Joint angle changes == 80 | # 81 | 82 | 83 | def calculate_mjac_deg(x: torch.Tensor) -> float: 84 | """Calculate the maximum change in configuration space over a path in configuration space.""" 85 | return torch.rad2deg(angular_changes(x).abs().max()).item() 86 | 87 | 88 | def calculate_per_timestep_mjac_deg(x: torch.Tensor) -> torch.Tensor: 89 | """Calculate the maximum change in configuration space over a path in configuration space.""" 90 | return torch.max(torch.rad2deg(angular_changes(x).abs()), dim=1).values 91 | 92 | 93 | def calculate_per_timestep_mjac_cm(x: torch.Tensor) -> torch.Tensor: 94 | return 100 * torch.max(prismatic_changes(x).abs(), dim=1).values 95 | 96 | 97 | def prismatic_changes(x: torch.Tensor) -> torch.Tensor: 98 | return x[1:] - x[0:-1] 99 | 100 | 101 | def get_mjacs(robot: Robot, qpath: torch.Tensor): 102 | qps_revolute, qps_prismatic = robot.split_configs_to_revolute_and_prismatic(qpath) 103 | if qps_prismatic.numel() > 0: 104 | return calculate_mjac_deg(qps_revolute), calculate_per_timestep_mjac_cm(qps_prismatic).abs().max().item() 105 | return calculate_mjac_deg(qps_revolute), 0.0 106 | 107 | 108 | # ====================== 109 | # == Pose error == 110 | # 111 | 112 | 113 | def calculate_pose_error_cm_deg(robot: Robot, x: torch.Tensor, target_path: torch.Tensor): 114 | """Calculate the positional and rotational errors in cm and rad of a config path""" 115 | traced_path = robot.forward_kinematics(x, out_device=DEVICE) 116 | return 100 * positional_errors(target_path, traced_path), torch.rad2deg(rotational_errors(target_path, traced_path)) 117 | 118 | 119 | def calculate_pose_error_mm_deg_and_mjac_cm_deg(robot: Robot, x: torch.Tensor, target_path: torch.Tensor): 120 | """Calculate the positional and rotational errors in mm of a config path""" 121 | traced_path = robot.forward_kinematics(x, out_device=DEVICE) 122 | x_revolute, x_prismatic = robot.split_configs_to_revolute_and_prismatic(x) 123 | mjac_cm = 0.0 124 | if x_prismatic.numel() > 0: 125 | mjac_cm = float(100 * x_prismatic.abs().max()) 126 | return ( 127 | 1000 * positional_errors(target_path, traced_path), 128 | torch.rad2deg(rotational_errors(target_path, traced_path)), 129 | float(torch.rad2deg(angular_changes(x_revolute).abs().max())), 130 | mjac_cm, 131 | ) 132 | 133 | 134 | def positional_errors(path_1: torch.Tensor, path_2: torch.Tensor) -> torch.Tensor: 135 | """Return the positional errors between two pose paths""" 136 | return torch.norm(path_1[:, :3] - path_2[:, :3], dim=1) 137 | 138 | 139 | def rotational_errors(path_1: torch.Tensor, path_2: torch.Tensor) -> torch.Tensor: 140 | """Computes the summed rotational error between two cartesian space paths.""" 141 | return geodesic_distance_between_quaternions(path_1[:, 3:], path_2[:, 3:]) 142 | 143 | 144 | def angular_changes(qpath: torch.Tensor) -> torch.Tensor: 145 | """Computes the change in the configuration space path. Respects jumps from 0 <-> 2pi 146 | 147 | WARNING: Results may be negative. Be sure to call .abs() if calculating the maximum absolute joint angle change 148 | 149 | Returns: a [n x ndof] array of the change in each joint angle over the n timesteps. 150 | """ 151 | dqs = qpath[1:] - qpath[0:-1] 152 | if isinstance(qpath, torch.Tensor): 153 | return torch.remainder(dqs + torch.pi, 2 * torch.pi) - torch.pi 154 | return np.remainder(dqs + np.pi, 2 * np.pi) - np.pi 155 | -------------------------------------------------------------------------------- /cppflow/lm_hyper_parameters.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | from dataclasses import dataclass 3 | import warnings 4 | 5 | import torch 6 | 7 | from cppflow.utils import Hashable 8 | 9 | ALTERNATING_LOSS_MAX_N_STEPS = 20 10 | ALTERNATING_LOSS_RETURN_IF_SOL_FOUND_AFTER = 15 11 | ALTERNATING_LOSS_CONVERGENCE_THRESHOLD = 0.3 12 | 13 | 14 | @dataclass 15 | class OptimizationParameters(Hashable): 16 | """Parameters for the optimizer""" 17 | 18 | seed_w_only_pose: bool 19 | lm_lambda: float 20 | 21 | # --- Alphas 22 | alpha_position: float 23 | alpha_rotation: float 24 | alpha_differencing: float 25 | alpha_differencing_prismatic_scaling: float 26 | # note: 'alpha_virtual_configs' multiplies 'alpha_differencing' to get the final scaling term 27 | alpha_virtual_configs: float 28 | alpha_self_collision: float 29 | alpha_env_collision: float 30 | 31 | # --- Pose error 32 | use_pose: bool 33 | pose_do_scale_down_satisfied: bool 34 | pose_ignore_satisfied_threshold_scale: float 35 | pose_ignore_satisfied_scale_down: float 36 | 37 | # --- Differencing error 38 | use_differencing: bool 39 | differencing_do_ignore_satisfied: bool 40 | differencing_ignore_satisfied_margin_deg: float 41 | differencing_ignore_satisfied_margin_cm: float 42 | # scale down satisfied differencing errors instead of filtering them out 43 | differencing_do_scale_satisfied: bool 44 | differencing_scale_down_satisfied_scale: float 45 | differencing_scale_down_satisfied_shift_invalid_to_threshold: ( 46 | bool # TODO: test out differencing_scale_down_satisfied_shift_invalid_to_threshold 47 | ) 48 | 49 | # --- Virtual configs 50 | use_virtual_configs: bool 51 | virtual_configs: Optional[torch.Tensor] 52 | n_virtual_configs: int 53 | 54 | # --- Collisions 55 | use_self_collisions: bool 56 | use_env_collisions: bool 57 | 58 | def __post_init__(self): 59 | if self.differencing_do_scale_satisfied and not self.use_virtual_configs: 60 | warnings.warn( 61 | "differencing_do_scale_satisfied is True, but virtual_configs are disabled. Using virtual configs with" 62 | " do_scale_satisfied is recommended - otherwise the differencing residual for configs at start/end will" 63 | " be unbalanced." 64 | ) 65 | if self.use_differencing: 66 | assert not ( 67 | self.differencing_do_ignore_satisfied and self.differencing_do_scale_satisfied 68 | ), "use one or the other, not both" 69 | if self.differencing_do_ignore_satisfied or self.differencing_do_scale_satisfied: 70 | assert self.differencing_ignore_satisfied_margin_deg > 0 71 | assert self.differencing_ignore_satisfied_margin_cm > 0 72 | if self.use_virtual_configs: 73 | assert self.virtual_configs is not None 74 | assert isinstance(self.n_virtual_configs, int) and self.n_virtual_configs > 0 75 | if self.use_self_collisions: 76 | assert self.alpha_self_collision > 0 77 | if self.use_env_collisions: 78 | assert self.alpha_env_collision > 0 79 | if self.pose_do_scale_down_satisfied: 80 | assert isinstance(self.pose_ignore_satisfied_threshold_scale, float) 81 | assert self.pose_ignore_satisfied_threshold_scale > 0 82 | 83 | 84 | # NOTE: parameters expect 1.5deg, 3cm padding in dp_search 85 | # includes tweaks compared to V2 86 | ALT_LOSS_V2_1_DIFF = OptimizationParameters( 87 | # General 88 | seed_w_only_pose=None, 89 | lm_lambda=1e-06, # 1e-6 seems to be optimal. higher or lower has worse performance 90 | alpha_position=None, 91 | alpha_rotation=None, 92 | # alpha_differencing=0.005, # too high for fetch__hello 93 | alpha_differencing=0.00375, 94 | # alpha_differencing=0.0025, 95 | alpha_differencing_prismatic_scaling=1.0, 96 | alpha_virtual_configs=1.0, 97 | alpha_self_collision=0.01, 98 | alpha_env_collision=0.01, 99 | # --- Pose 100 | use_pose=False, 101 | pose_do_scale_down_satisfied=False, 102 | pose_ignore_satisfied_threshold_scale=None, 103 | pose_ignore_satisfied_scale_down=None, 104 | # --- Differencing 105 | use_differencing=True, 106 | differencing_do_ignore_satisfied=False, 107 | differencing_ignore_satisfied_margin_deg=None, 108 | differencing_ignore_satisfied_margin_cm=None, 109 | differencing_do_scale_satisfied=False, 110 | differencing_scale_down_satisfied_scale=None, 111 | differencing_scale_down_satisfied_shift_invalid_to_threshold=None, 112 | # --- Virtual Configs, collisions 113 | use_virtual_configs=True, # need virtual configs when differencing_ignore_satisfied=False 114 | virtual_configs=torch.tensor([]), 115 | n_virtual_configs=4, 116 | use_self_collisions=True, 117 | use_env_collisions=True, 118 | ) 119 | ALT_LOSS_V2_1_POSE = OptimizationParameters( 120 | # General 121 | seed_w_only_pose=None, 122 | # lm_lambda=1e-08, # this causes bad convergence with fetch__hello 123 | lm_lambda=1e-06, 124 | # --- Alphas 125 | alpha_position=3.5, 126 | alpha_rotation=0.35, 127 | alpha_differencing=None, 128 | alpha_differencing_prismatic_scaling=None, 129 | alpha_virtual_configs=None, 130 | alpha_self_collision=None, 131 | alpha_env_collision=None, 132 | # --- Pose 133 | use_pose=True, 134 | pose_do_scale_down_satisfied=False, 135 | pose_ignore_satisfied_threshold_scale=None, 136 | pose_ignore_satisfied_scale_down=None, 137 | # --- Differencing 138 | use_differencing=False, 139 | differencing_do_ignore_satisfied=False, 140 | differencing_ignore_satisfied_margin_deg=None, 141 | differencing_ignore_satisfied_margin_cm=None, 142 | differencing_do_scale_satisfied=False, 143 | differencing_scale_down_satisfied_scale=None, 144 | differencing_scale_down_satisfied_shift_invalid_to_threshold=True, 145 | # --- Virtual Configs, collisions 146 | use_virtual_configs=False, 147 | virtual_configs=None, 148 | n_virtual_configs=None, 149 | use_self_collisions=False, 150 | use_env_collisions=False, 151 | ) 152 | -------------------------------------------------------------------------------- /cppflow/paths/1cube.csv: -------------------------------------------------------------------------------- 1 | ,0,1,2,3,4,5,6 2 | 0,0.45,0.0,0.0,1.0,0.0,0.0,0.0 3 | 1,0.44547737,0.0,0.0,1.0,0.0,0.0,0.0 4 | 2,0.44095477,0.0,0.0,1.0,0.0,0.0,0.0 5 | 3,0.43643215,0.0,0.0,1.0,0.0,0.0,0.0 6 | 4,0.43190953,0.0,0.0,1.0,0.0,0.0,0.0 7 | 5,0.4273869,0.0,0.0,1.0,0.0,0.0,0.0 8 | 6,0.42286432,0.0,0.0,1.0,0.0,0.0,0.0 9 | 7,0.4183417,0.0,0.0,1.0,0.0,0.0,0.0 10 | 8,0.41381907,0.0,0.0,1.0,0.0,0.0,0.0 11 | 9,0.40929648,0.0,0.0,1.0,0.0,0.0,0.0 12 | 10,0.40477386,0.0,0.0,1.0,0.0,0.0,0.0 13 | 11,0.40025124,0.0,0.0,1.0,0.0,0.0,0.0 14 | 12,0.39572865,0.0,0.0,1.0,0.0,0.0,0.0 15 | 13,0.39120603,0.0,0.0,1.0,0.0,0.0,0.0 16 | 14,0.3866834,0.0,0.0,1.0,0.0,0.0,0.0 17 | 15,0.38216078,0.0,0.0,1.0,0.0,0.0,0.0 18 | 16,0.3776382,0.0,0.0,1.0,0.0,0.0,0.0 19 | 17,0.37311557,0.0,0.0,1.0,0.0,0.0,0.0 20 | 18,0.36859295,0.0,0.0,1.0,0.0,0.0,0.0 21 | 19,0.36407036,0.0,0.0,1.0,0.0,0.0,0.0 22 | 20,0.35954773,0.0,0.0,1.0,0.0,0.0,0.0 23 | 21,0.3550251,0.0,0.0,1.0,0.0,0.0,0.0 24 | 22,0.35050252,0.0,0.0,1.0,0.0,0.0,0.0 25 | 23,0.3459799,0.0,0.0,1.0,0.0,0.0,0.0 26 | 24,0.34145728,0.0,0.0,1.0,0.0,0.0,0.0 27 | 25,0.33693466,0.0,0.0,1.0,0.0,0.0,0.0 28 | 26,0.33241206,0.0,0.0,1.0,0.0,0.0,0.0 29 | 27,0.32788944,0.0,0.0,1.0,0.0,0.0,0.0 30 | 28,0.32336682,0.0,0.0,1.0,0.0,0.0,0.0 31 | 29,0.31884423,0.0,0.0,1.0,0.0,0.0,0.0 32 | 30,0.3143216,0.0,0.0,1.0,0.0,0.0,0.0 33 | 31,0.309799,0.0,0.0,1.0,0.0,0.0,0.0 34 | 32,0.3052764,0.0,0.0,1.0,0.0,0.0,0.0 35 | 33,0.30075377,0.0,0.0,1.0,0.0,0.0,0.0 36 | 34,0.29623115,0.0,0.0,1.0,0.0,0.0,0.0 37 | 35,0.29170853,0.0,0.0,1.0,0.0,0.0,0.0 38 | 36,0.28718594,0.0,0.0,1.0,0.0,0.0,0.0 39 | 37,0.28266332,0.0,0.0,1.0,0.0,0.0,0.0 40 | 38,0.2781407,0.0,0.0,1.0,0.0,0.0,0.0 41 | 39,0.2736181,0.0,0.0,1.0,0.0,0.0,0.0 42 | 40,0.26909548,0.0,0.0,1.0,0.0,0.0,0.0 43 | 41,0.26457286,0.0,0.0,1.0,0.0,0.0,0.0 44 | 42,0.26005024,0.0,0.0,1.0,0.0,0.0,0.0 45 | 43,0.25552765,0.0,0.0,1.0,0.0,0.0,0.0 46 | 44,0.25100502,0.0,0.0,1.0,0.0,0.0,0.0 47 | 45,0.24648242,0.0,0.0,1.0,0.0,0.0,0.0 48 | 46,0.2419598,0.0,0.0,1.0,0.0,0.0,0.0 49 | 47,0.23743719,0.0,0.0,1.0,0.0,0.0,0.0 50 | 48,0.23291457,0.0,0.0,1.0,0.0,0.0,0.0 51 | 49,0.22839196,0.0,0.0,1.0,0.0,0.0,0.0 52 | 50,0.22386935,0.0,0.0,1.0,0.0,0.0,0.0 53 | 51,0.21934673,0.0,0.0,1.0,0.0,0.0,0.0 54 | 52,0.21482413,0.0,0.0,1.0,0.0,0.0,0.0 55 | 53,0.2103015,0.0,0.0,1.0,0.0,0.0,0.0 56 | 54,0.2057789,0.0,0.0,1.0,0.0,0.0,0.0 57 | 55,0.20125629,0.0,0.0,1.0,0.0,0.0,0.0 58 | 56,0.19673367,0.0,0.0,1.0,0.0,0.0,0.0 59 | 57,0.19221106,0.0,0.0,1.0,0.0,0.0,0.0 60 | 58,0.18768844,0.0,0.0,1.0,0.0,0.0,0.0 61 | 59,0.18316583,0.0,0.0,1.0,0.0,0.0,0.0 62 | 60,0.17864323,0.0,0.0,1.0,0.0,0.0,0.0 63 | 61,0.1741206,0.0,0.0,1.0,0.0,0.0,0.0 64 | 62,0.169598,0.0,0.0,1.0,0.0,0.0,0.0 65 | 63,0.16507538,0.0,0.0,1.0,0.0,0.0,0.0 66 | 64,0.16055277,0.0,0.0,1.0,0.0,0.0,0.0 67 | 65,0.15603016,0.0,0.0,1.0,0.0,0.0,0.0 68 | 66,0.15150754,0.0,0.0,1.0,0.0,0.0,0.0 69 | 67,0.14698493,0.0,0.0,1.0,0.0,0.0,0.0 70 | 68,0.14246231,0.0,0.0,1.0,0.0,0.0,0.0 71 | 69,0.1379397,0.0,0.0,1.0,0.0,0.0,0.0 72 | 70,0.1334171,0.0,0.0,1.0,0.0,0.0,0.0 73 | 71,0.12889448,0.0,0.0,1.0,0.0,0.0,0.0 74 | 72,0.12437187,0.0,0.0,1.0,0.0,0.0,0.0 75 | 73,0.11984926,0.0,0.0,1.0,0.0,0.0,0.0 76 | 74,0.11532664,0.0,0.0,1.0,0.0,0.0,0.0 77 | 75,0.11080403,0.0,0.0,1.0,0.0,0.0,0.0 78 | 76,0.106281415,0.0,0.0,1.0,0.0,0.0,0.0 79 | 77,0.1017588,0.0,0.0,1.0,0.0,0.0,0.0 80 | 78,0.09723619,0.0,0.0,1.0,0.0,0.0,0.0 81 | 79,0.09271358,0.0,0.0,1.0,0.0,0.0,0.0 82 | 80,0.088190965,0.0,0.0,1.0,0.0,0.0,0.0 83 | 81,0.08366835,0.0,0.0,1.0,0.0,0.0,0.0 84 | 82,0.07914574,0.0,0.0,1.0,0.0,0.0,0.0 85 | 83,0.07462313,0.0,0.0,1.0,0.0,0.0,0.0 86 | 84,0.070100516,0.0,0.0,1.0,0.0,0.0,0.0 87 | 85,0.0655779,0.0,0.0,1.0,0.0,0.0,0.0 88 | 86,0.061055288,0.0,0.0,1.0,0.0,0.0,0.0 89 | 87,0.056532677,0.0,0.0,1.0,0.0,0.0,0.0 90 | 88,0.052010063,0.0,0.0,1.0,0.0,0.0,0.0 91 | 89,0.04748745,0.0,0.0,1.0,0.0,0.0,0.0 92 | 90,0.04296484,0.0,0.0,1.0,0.0,0.0,0.0 93 | 91,0.038442224,0.0,0.0,1.0,0.0,0.0,0.0 94 | 92,0.03391961,0.0,0.0,1.0,0.0,0.0,0.0 95 | 93,0.029397,0.0,0.0,1.0,0.0,0.0,0.0 96 | 94,0.024874385,0.0,0.0,1.0,0.0,0.0,0.0 97 | 95,0.020351773,0.0,0.0,1.0,0.0,0.0,0.0 98 | 96,0.01582916,0.0,0.0,1.0,0.0,0.0,0.0 99 | 97,0.0113065485,0.0,0.0,1.0,0.0,0.0,0.0 100 | 98,0.0067839352,0.0,0.0,1.0,0.0,0.0,0.0 101 | 99,0.0022613225,0.0,0.0,1.0,0.0,0.0,0.0 102 | 100,-0.0022613225,0.0,0.0,1.0,0.0,0.0,0.0 103 | 101,-0.0067839352,0.0,0.0,1.0,0.0,0.0,0.0 104 | 102,-0.0113065485,0.0,0.0,1.0,0.0,0.0,0.0 105 | 103,-0.01582916,0.0,0.0,1.0,0.0,0.0,0.0 106 | 104,-0.020351773,0.0,0.0,1.0,0.0,0.0,0.0 107 | 105,-0.024874385,0.0,0.0,1.0,0.0,0.0,0.0 108 | 106,-0.029397,0.0,0.0,1.0,0.0,0.0,0.0 109 | 107,-0.03391961,0.0,0.0,1.0,0.0,0.0,0.0 110 | 108,-0.038442224,0.0,0.0,1.0,0.0,0.0,0.0 111 | 109,-0.04296484,0.0,0.0,1.0,0.0,0.0,0.0 112 | 110,-0.04748745,0.0,0.0,1.0,0.0,0.0,0.0 113 | 111,-0.052010063,0.0,0.0,1.0,0.0,0.0,0.0 114 | 112,-0.056532677,0.0,0.0,1.0,0.0,0.0,0.0 115 | 113,-0.061055288,0.0,0.0,1.0,0.0,0.0,0.0 116 | 114,-0.0655779,0.0,0.0,1.0,0.0,0.0,0.0 117 | 115,-0.070100516,0.0,0.0,1.0,0.0,0.0,0.0 118 | 116,-0.07462313,0.0,0.0,1.0,0.0,0.0,0.0 119 | 117,-0.07914574,0.0,0.0,1.0,0.0,0.0,0.0 120 | 118,-0.08366835,0.0,0.0,1.0,0.0,0.0,0.0 121 | 119,-0.088190965,0.0,0.0,1.0,0.0,0.0,0.0 122 | 120,-0.09271358,0.0,0.0,1.0,0.0,0.0,0.0 123 | 121,-0.09723619,0.0,0.0,1.0,0.0,0.0,0.0 124 | 122,-0.1017588,0.0,0.0,1.0,0.0,0.0,0.0 125 | 123,-0.106281415,0.0,0.0,1.0,0.0,0.0,0.0 126 | 124,-0.11080403,0.0,0.0,1.0,0.0,0.0,0.0 127 | 125,-0.11532664,0.0,0.0,1.0,0.0,0.0,0.0 128 | 126,-0.11984926,0.0,0.0,1.0,0.0,0.0,0.0 129 | 127,-0.12437187,0.0,0.0,1.0,0.0,0.0,0.0 130 | 128,-0.12889448,0.0,0.0,1.0,0.0,0.0,0.0 131 | 129,-0.1334171,0.0,0.0,1.0,0.0,0.0,0.0 132 | 130,-0.1379397,0.0,0.0,1.0,0.0,0.0,0.0 133 | 131,-0.14246231,0.0,0.0,1.0,0.0,0.0,0.0 134 | 132,-0.14698493,0.0,0.0,1.0,0.0,0.0,0.0 135 | 133,-0.15150754,0.0,0.0,1.0,0.0,0.0,0.0 136 | 134,-0.15603016,0.0,0.0,1.0,0.0,0.0,0.0 137 | 135,-0.16055277,0.0,0.0,1.0,0.0,0.0,0.0 138 | 136,-0.16507538,0.0,0.0,1.0,0.0,0.0,0.0 139 | 137,-0.169598,0.0,0.0,1.0,0.0,0.0,0.0 140 | 138,-0.1741206,0.0,0.0,1.0,0.0,0.0,0.0 141 | 139,-0.17864323,0.0,0.0,1.0,0.0,0.0,0.0 142 | 140,-0.18316583,0.0,0.0,1.0,0.0,0.0,0.0 143 | 141,-0.18768844,0.0,0.0,1.0,0.0,0.0,0.0 144 | 142,-0.19221106,0.0,0.0,1.0,0.0,0.0,0.0 145 | 143,-0.19673367,0.0,0.0,1.0,0.0,0.0,0.0 146 | 144,-0.20125629,0.0,0.0,1.0,0.0,0.0,0.0 147 | 145,-0.2057789,0.0,0.0,1.0,0.0,0.0,0.0 148 | 146,-0.2103015,0.0,0.0,1.0,0.0,0.0,0.0 149 | 147,-0.21482413,0.0,0.0,1.0,0.0,0.0,0.0 150 | 148,-0.21934673,0.0,0.0,1.0,0.0,0.0,0.0 151 | 149,-0.22386935,0.0,0.0,1.0,0.0,0.0,0.0 152 | 150,-0.22839196,0.0,0.0,1.0,0.0,0.0,0.0 153 | 151,-0.23291457,0.0,0.0,1.0,0.0,0.0,0.0 154 | 152,-0.23743719,0.0,0.0,1.0,0.0,0.0,0.0 155 | 153,-0.2419598,0.0,0.0,1.0,0.0,0.0,0.0 156 | 154,-0.24648242,0.0,0.0,1.0,0.0,0.0,0.0 157 | 155,-0.25100502,0.0,0.0,1.0,0.0,0.0,0.0 158 | 156,-0.25552765,0.0,0.0,1.0,0.0,0.0,0.0 159 | 157,-0.26005024,0.0,0.0,1.0,0.0,0.0,0.0 160 | 158,-0.26457286,0.0,0.0,1.0,0.0,0.0,0.0 161 | 159,-0.26909548,0.0,0.0,1.0,0.0,0.0,0.0 162 | 160,-0.2736181,0.0,0.0,1.0,0.0,0.0,0.0 163 | 161,-0.2781407,0.0,0.0,1.0,0.0,0.0,0.0 164 | 162,-0.28266332,0.0,0.0,1.0,0.0,0.0,0.0 165 | 163,-0.28718594,0.0,0.0,1.0,0.0,0.0,0.0 166 | 164,-0.29170853,0.0,0.0,1.0,0.0,0.0,0.0 167 | 165,-0.29623115,0.0,0.0,1.0,0.0,0.0,0.0 168 | 166,-0.30075377,0.0,0.0,1.0,0.0,0.0,0.0 169 | 167,-0.3052764,0.0,0.0,1.0,0.0,0.0,0.0 170 | 168,-0.309799,0.0,0.0,1.0,0.0,0.0,0.0 171 | 169,-0.3143216,0.0,0.0,1.0,0.0,0.0,0.0 172 | 170,-0.31884423,0.0,0.0,1.0,0.0,0.0,0.0 173 | 171,-0.32336682,0.0,0.0,1.0,0.0,0.0,0.0 174 | 172,-0.32788944,0.0,0.0,1.0,0.0,0.0,0.0 175 | 173,-0.33241206,0.0,0.0,1.0,0.0,0.0,0.0 176 | 174,-0.33693466,0.0,0.0,1.0,0.0,0.0,0.0 177 | 175,-0.34145728,0.0,0.0,1.0,0.0,0.0,0.0 178 | 176,-0.3459799,0.0,0.0,1.0,0.0,0.0,0.0 179 | 177,-0.35050252,0.0,0.0,1.0,0.0,0.0,0.0 180 | 178,-0.3550251,0.0,0.0,1.0,0.0,0.0,0.0 181 | 179,-0.35954773,0.0,0.0,1.0,0.0,0.0,0.0 182 | 180,-0.36407036,0.0,0.0,1.0,0.0,0.0,0.0 183 | 181,-0.36859295,0.0,0.0,1.0,0.0,0.0,0.0 184 | 182,-0.37311557,0.0,0.0,1.0,0.0,0.0,0.0 185 | 183,-0.3776382,0.0,0.0,1.0,0.0,0.0,0.0 186 | 184,-0.38216078,0.0,0.0,1.0,0.0,0.0,0.0 187 | 185,-0.3866834,0.0,0.0,1.0,0.0,0.0,0.0 188 | 186,-0.39120603,0.0,0.0,1.0,0.0,0.0,0.0 189 | 187,-0.39572865,0.0,0.0,1.0,0.0,0.0,0.0 190 | 188,-0.40025124,0.0,0.0,1.0,0.0,0.0,0.0 191 | 189,-0.40477386,0.0,0.0,1.0,0.0,0.0,0.0 192 | 190,-0.40929648,0.0,0.0,1.0,0.0,0.0,0.0 193 | 191,-0.41381907,0.0,0.0,1.0,0.0,0.0,0.0 194 | 192,-0.4183417,0.0,0.0,1.0,0.0,0.0,0.0 195 | 193,-0.42286432,0.0,0.0,1.0,0.0,0.0,0.0 196 | 194,-0.4273869,0.0,0.0,1.0,0.0,0.0,0.0 197 | 195,-0.43190953,0.0,0.0,1.0,0.0,0.0,0.0 198 | 196,-0.43643215,0.0,0.0,1.0,0.0,0.0,0.0 199 | 197,-0.44095477,0.0,0.0,1.0,0.0,0.0,0.0 200 | 198,-0.44547737,0.0,0.0,1.0,0.0,0.0,0.0 201 | 199,-0.45,0.0,0.0,1.0,0.0,0.0,0.0 202 | -------------------------------------------------------------------------------- /cppflow/paths/1cube_mini.csv: -------------------------------------------------------------------------------- 1 | ,0,1,2,3,4,5,6 2 | 0,0.45,0.0,0.0,1.0,0.0,0.0,0.0 3 | 1,0.44547737,0.0,0.0,1.0,0.0,0.0,0.0 4 | 2,0.44095477,0.0,0.0,1.0,0.0,0.0,0.0 5 | 3,0.43643215,0.0,0.0,1.0,0.0,0.0,0.0 6 | 4,0.43190953,0.0,0.0,1.0,0.0,0.0,0.0 7 | 5,0.4273869,0.0,0.0,1.0,0.0,0.0,0.0 8 | 6,0.42286432,0.0,0.0,1.0,0.0,0.0,0.0 9 | 7,0.4183417,0.0,0.0,1.0,0.0,0.0,0.0 10 | 8,0.41381907,0.0,0.0,1.0,0.0,0.0,0.0 11 | 9,0.40929648,0.0,0.0,1.0,0.0,0.0,0.0 12 | 10,0.40477386,0.0,0.0,1.0,0.0,0.0,0.0 13 | 11,0.40025124,0.0,0.0,1.0,0.0,0.0,0.0 14 | 12,0.39572865,0.0,0.0,1.0,0.0,0.0,0.0 15 | 13,0.39120603,0.0,0.0,1.0,0.0,0.0,0.0 16 | 14,0.3866834,0.0,0.0,1.0,0.0,0.0,0.0 17 | 15,0.38216078,0.0,0.0,1.0,0.0,0.0,0.0 18 | 16,0.3776382,0.0,0.0,1.0,0.0,0.0,0.0 19 | 17,0.37311557,0.0,0.0,1.0,0.0,0.0,0.0 20 | 18,0.36859295,0.0,0.0,1.0,0.0,0.0,0.0 21 | 19,0.36407036,0.0,0.0,1.0,0.0,0.0,0.0 22 | 20,0.35954773,0.0,0.0,1.0,0.0,0.0,0.0 23 | 21,0.3550251,0.0,0.0,1.0,0.0,0.0,0.0 24 | 22,0.35050252,0.0,0.0,1.0,0.0,0.0,0.0 25 | 23,0.3459799,0.0,0.0,1.0,0.0,0.0,0.0 26 | 24,0.34145728,0.0,0.0,1.0,0.0,0.0,0.0 27 | -------------------------------------------------------------------------------- /cppflow/paths/2cubes.csv: -------------------------------------------------------------------------------- 1 | time,x,y,z,qw,qx,qy,qz 2 | 0,0.45,0.0,0.0,1.0,0.0,0.0,0.0 3 | 1,0.44547737,0.0,0.0,1.0,0.0,0.0,0.0 4 | 2,0.44095477,0.0,0.0,1.0,0.0,0.0,0.0 5 | 3,0.43643215,0.0,0.0,1.0,0.0,0.0,0.0 6 | 4,0.43190953,0.0,0.0,1.0,0.0,0.0,0.0 7 | 5,0.4273869,0.0,0.0,1.0,0.0,0.0,0.0 8 | 6,0.42286432,0.0,0.0,1.0,0.0,0.0,0.0 9 | 7,0.4183417,0.0,0.0,1.0,0.0,0.0,0.0 10 | 8,0.41381907,0.0,0.0,1.0,0.0,0.0,0.0 11 | 9,0.40929648,0.0,0.0,1.0,0.0,0.0,0.0 12 | 10,0.40477386,0.0,0.0,1.0,0.0,0.0,0.0 13 | 11,0.40025124,0.0,0.0,1.0,0.0,0.0,0.0 14 | 12,0.39572865,0.0,0.0,1.0,0.0,0.0,0.0 15 | 13,0.39120603,0.0,0.0,1.0,0.0,0.0,0.0 16 | 14,0.3866834,0.0,0.0,1.0,0.0,0.0,0.0 17 | 15,0.38216078,0.0,0.0,1.0,0.0,0.0,0.0 18 | 16,0.3776382,0.0,0.0,1.0,0.0,0.0,0.0 19 | 17,0.37311557,0.0,0.0,1.0,0.0,0.0,0.0 20 | 18,0.36859295,0.0,0.0,1.0,0.0,0.0,0.0 21 | 19,0.36407036,0.0,0.0,1.0,0.0,0.0,0.0 22 | 20,0.35954773,0.0,0.0,1.0,0.0,0.0,0.0 23 | 21,0.3550251,0.0,0.0,1.0,0.0,0.0,0.0 24 | 22,0.35050252,0.0,0.0,1.0,0.0,0.0,0.0 25 | 23,0.3459799,0.0,0.0,1.0,0.0,0.0,0.0 26 | 24,0.34145728,0.0,0.0,1.0,0.0,0.0,0.0 27 | 25,0.33693466,0.0,0.0,1.0,0.0,0.0,0.0 28 | 26,0.33241206,0.0,0.0,1.0,0.0,0.0,0.0 29 | 27,0.32788944,0.0,0.0,1.0,0.0,0.0,0.0 30 | 28,0.32336682,0.0,0.0,1.0,0.0,0.0,0.0 31 | 29,0.31884423,0.0,0.0,1.0,0.0,0.0,0.0 32 | 30,0.3143216,0.0,0.0,1.0,0.0,0.0,0.0 33 | 31,0.309799,0.0,0.0,1.0,0.0,0.0,0.0 34 | 32,0.3052764,0.0,0.0,1.0,0.0,0.0,0.0 35 | 33,0.30075377,0.0,0.0,1.0,0.0,0.0,0.0 36 | 34,0.29623115,0.0,0.0,1.0,0.0,0.0,0.0 37 | 35,0.29170853,0.0,0.0,1.0,0.0,0.0,0.0 38 | 36,0.28718594,0.0,0.0,1.0,0.0,0.0,0.0 39 | 37,0.28266332,0.0,0.0,1.0,0.0,0.0,0.0 40 | 38,0.2781407,0.0,0.0,1.0,0.0,0.0,0.0 41 | 39,0.2736181,0.0,0.0,1.0,0.0,0.0,0.0 42 | 40,0.26909548,0.0,0.0,1.0,0.0,0.0,0.0 43 | 41,0.26457286,0.0,0.0,1.0,0.0,0.0,0.0 44 | 42,0.26005024,0.0,0.0,1.0,0.0,0.0,0.0 45 | 43,0.25552765,0.0,0.0,1.0,0.0,0.0,0.0 46 | 44,0.25100502,0.0,0.0,1.0,0.0,0.0,0.0 47 | 45,0.24648242,0.0,0.0,1.0,0.0,0.0,0.0 48 | 46,0.2419598,0.0,0.0,1.0,0.0,0.0,0.0 49 | 47,0.23743719,0.0,0.0,1.0,0.0,0.0,0.0 50 | 48,0.23291457,0.0,0.0,1.0,0.0,0.0,0.0 51 | 49,0.22839196,0.0,0.0,1.0,0.0,0.0,0.0 52 | 50,0.22386935,0.0,0.0,1.0,0.0,0.0,0.0 53 | 51,0.21934673,0.0,0.0,1.0,0.0,0.0,0.0 54 | 52,0.21482413,0.0,0.0,1.0,0.0,0.0,0.0 55 | 53,0.2103015,0.0,0.0,1.0,0.0,0.0,0.0 56 | 54,0.2057789,0.0,0.0,1.0,0.0,0.0,0.0 57 | 55,0.20125629,0.0,0.0,1.0,0.0,0.0,0.0 58 | 56,0.19673367,0.0,0.0,1.0,0.0,0.0,0.0 59 | 57,0.19221106,0.0,0.0,1.0,0.0,0.0,0.0 60 | 58,0.18768844,0.0,0.0,1.0,0.0,0.0,0.0 61 | 59,0.18316583,0.0,0.0,1.0,0.0,0.0,0.0 62 | 60,0.17864323,0.0,0.0,1.0,0.0,0.0,0.0 63 | 61,0.1741206,0.0,0.0,1.0,0.0,0.0,0.0 64 | 62,0.169598,0.0,0.0,1.0,0.0,0.0,0.0 65 | 63,0.16507538,0.0,0.0,1.0,0.0,0.0,0.0 66 | 64,0.16055277,0.0,0.0,1.0,0.0,0.0,0.0 67 | 65,0.15603016,0.0,0.0,1.0,0.0,0.0,0.0 68 | 66,0.15150754,0.0,0.0,1.0,0.0,0.0,0.0 69 | 67,0.14698493,0.0,0.0,1.0,0.0,0.0,0.0 70 | 68,0.14246231,0.0,0.0,1.0,0.0,0.0,0.0 71 | 69,0.1379397,0.0,0.0,1.0,0.0,0.0,0.0 72 | 70,0.1334171,0.0,0.0,1.0,0.0,0.0,0.0 73 | 71,0.12889448,0.0,0.0,1.0,0.0,0.0,0.0 74 | 72,0.12437187,0.0,0.0,1.0,0.0,0.0,0.0 75 | 73,0.11984926,0.0,0.0,1.0,0.0,0.0,0.0 76 | 74,0.11532664,0.0,0.0,1.0,0.0,0.0,0.0 77 | 75,0.11080403,0.0,0.0,1.0,0.0,0.0,0.0 78 | 76,0.106281415,0.0,0.0,1.0,0.0,0.0,0.0 79 | 77,0.1017588,0.0,0.0,1.0,0.0,0.0,0.0 80 | 78,0.09723619,0.0,0.0,1.0,0.0,0.0,0.0 81 | 79,0.09271358,0.0,0.0,1.0,0.0,0.0,0.0 82 | 80,0.088190965,0.0,0.0,1.0,0.0,0.0,0.0 83 | 81,0.08366835,0.0,0.0,1.0,0.0,0.0,0.0 84 | 82,0.07914574,0.0,0.0,1.0,0.0,0.0,0.0 85 | 83,0.07462313,0.0,0.0,1.0,0.0,0.0,0.0 86 | 84,0.070100516,0.0,0.0,1.0,0.0,0.0,0.0 87 | 85,0.0655779,0.0,0.0,1.0,0.0,0.0,0.0 88 | 86,0.061055288,0.0,0.0,1.0,0.0,0.0,0.0 89 | 87,0.056532677,0.0,0.0,1.0,0.0,0.0,0.0 90 | 88,0.052010063,0.0,0.0,1.0,0.0,0.0,0.0 91 | 89,0.04748745,0.0,0.0,1.0,0.0,0.0,0.0 92 | 90,0.04296484,0.0,0.0,1.0,0.0,0.0,0.0 93 | 91,0.038442224,0.0,0.0,1.0,0.0,0.0,0.0 94 | 92,0.03391961,0.0,0.0,1.0,0.0,0.0,0.0 95 | 93,0.029397,0.0,0.0,1.0,0.0,0.0,0.0 96 | 94,0.024874385,0.0,0.0,1.0,0.0,0.0,0.0 97 | 95,0.020351773,0.0,0.0,1.0,0.0,0.0,0.0 98 | 96,0.01582916,0.0,0.0,1.0,0.0,0.0,0.0 99 | 97,0.0113065485,0.0,0.0,1.0,0.0,0.0,0.0 100 | 98,0.0067839352,0.0,0.0,1.0,0.0,0.0,0.0 101 | 99,0.0022613225,0.0,0.0,1.0,0.0,0.0,0.0 102 | 100,-0.0022613225,0.0,0.0,1.0,0.0,0.0,0.0 103 | 101,-0.0067839352,0.0,0.0,1.0,0.0,0.0,0.0 104 | 102,-0.0113065485,0.0,0.0,1.0,0.0,0.0,0.0 105 | 103,-0.01582916,0.0,0.0,1.0,0.0,0.0,0.0 106 | 104,-0.020351773,0.0,0.0,1.0,0.0,0.0,0.0 107 | 105,-0.024874385,0.0,0.0,1.0,0.0,0.0,0.0 108 | 106,-0.029397,0.0,0.0,1.0,0.0,0.0,0.0 109 | 107,-0.03391961,0.0,0.0,1.0,0.0,0.0,0.0 110 | 108,-0.038442224,0.0,0.0,1.0,0.0,0.0,0.0 111 | 109,-0.04296484,0.0,0.0,1.0,0.0,0.0,0.0 112 | 110,-0.04748745,0.0,0.0,1.0,0.0,0.0,0.0 113 | 111,-0.052010063,0.0,0.0,1.0,0.0,0.0,0.0 114 | 112,-0.056532677,0.0,0.0,1.0,0.0,0.0,0.0 115 | 113,-0.061055288,0.0,0.0,1.0,0.0,0.0,0.0 116 | 114,-0.0655779,0.0,0.0,1.0,0.0,0.0,0.0 117 | 115,-0.070100516,0.0,0.0,1.0,0.0,0.0,0.0 118 | 116,-0.07462313,0.0,0.0,1.0,0.0,0.0,0.0 119 | 117,-0.07914574,0.0,0.0,1.0,0.0,0.0,0.0 120 | 118,-0.08366835,0.0,0.0,1.0,0.0,0.0,0.0 121 | 119,-0.088190965,0.0,0.0,1.0,0.0,0.0,0.0 122 | 120,-0.09271358,0.0,0.0,1.0,0.0,0.0,0.0 123 | 121,-0.09723619,0.0,0.0,1.0,0.0,0.0,0.0 124 | 122,-0.1017588,0.0,0.0,1.0,0.0,0.0,0.0 125 | 123,-0.106281415,0.0,0.0,1.0,0.0,0.0,0.0 126 | 124,-0.11080403,0.0,0.0,1.0,0.0,0.0,0.0 127 | 125,-0.11532664,0.0,0.0,1.0,0.0,0.0,0.0 128 | 126,-0.11984926,0.0,0.0,1.0,0.0,0.0,0.0 129 | 127,-0.12437187,0.0,0.0,1.0,0.0,0.0,0.0 130 | 128,-0.12889448,0.0,0.0,1.0,0.0,0.0,0.0 131 | 129,-0.1334171,0.0,0.0,1.0,0.0,0.0,0.0 132 | 130,-0.1379397,0.0,0.0,1.0,0.0,0.0,0.0 133 | 131,-0.14246231,0.0,0.0,1.0,0.0,0.0,0.0 134 | 132,-0.14698493,0.0,0.0,1.0,0.0,0.0,0.0 135 | 133,-0.15150754,0.0,0.0,1.0,0.0,0.0,0.0 136 | 134,-0.15603016,0.0,0.0,1.0,0.0,0.0,0.0 137 | 135,-0.16055277,0.0,0.0,1.0,0.0,0.0,0.0 138 | 136,-0.16507538,0.0,0.0,1.0,0.0,0.0,0.0 139 | 137,-0.169598,0.0,0.0,1.0,0.0,0.0,0.0 140 | 138,-0.1741206,0.0,0.0,1.0,0.0,0.0,0.0 141 | 139,-0.17864323,0.0,0.0,1.0,0.0,0.0,0.0 142 | 140,-0.18316583,0.0,0.0,1.0,0.0,0.0,0.0 143 | 141,-0.18768844,0.0,0.0,1.0,0.0,0.0,0.0 144 | 142,-0.19221106,0.0,0.0,1.0,0.0,0.0,0.0 145 | 143,-0.19673367,0.0,0.0,1.0,0.0,0.0,0.0 146 | 144,-0.20125629,0.0,0.0,1.0,0.0,0.0,0.0 147 | 145,-0.2057789,0.0,0.0,1.0,0.0,0.0,0.0 148 | 146,-0.2103015,0.0,0.0,1.0,0.0,0.0,0.0 149 | 147,-0.21482413,0.0,0.0,1.0,0.0,0.0,0.0 150 | 148,-0.21934673,0.0,0.0,1.0,0.0,0.0,0.0 151 | 149,-0.22386935,0.0,0.0,1.0,0.0,0.0,0.0 152 | 150,-0.22839196,0.0,0.0,1.0,0.0,0.0,0.0 153 | 151,-0.23291457,0.0,0.0,1.0,0.0,0.0,0.0 154 | 152,-0.23743719,0.0,0.0,1.0,0.0,0.0,0.0 155 | 153,-0.2419598,0.0,0.0,1.0,0.0,0.0,0.0 156 | 154,-0.24648242,0.0,0.0,1.0,0.0,0.0,0.0 157 | 155,-0.25100502,0.0,0.0,1.0,0.0,0.0,0.0 158 | 156,-0.25552765,0.0,0.0,1.0,0.0,0.0,0.0 159 | 157,-0.26005024,0.0,0.0,1.0,0.0,0.0,0.0 160 | 158,-0.26457286,0.0,0.0,1.0,0.0,0.0,0.0 161 | 159,-0.26909548,0.0,0.0,1.0,0.0,0.0,0.0 162 | 160,-0.2736181,0.0,0.0,1.0,0.0,0.0,0.0 163 | 161,-0.2781407,0.0,0.0,1.0,0.0,0.0,0.0 164 | 162,-0.28266332,0.0,0.0,1.0,0.0,0.0,0.0 165 | 163,-0.28718594,0.0,0.0,1.0,0.0,0.0,0.0 166 | 164,-0.29170853,0.0,0.0,1.0,0.0,0.0,0.0 167 | 165,-0.29623115,0.0,0.0,1.0,0.0,0.0,0.0 168 | 166,-0.30075377,0.0,0.0,1.0,0.0,0.0,0.0 169 | 167,-0.3052764,0.0,0.0,1.0,0.0,0.0,0.0 170 | 168,-0.309799,0.0,0.0,1.0,0.0,0.0,0.0 171 | 169,-0.3143216,0.0,0.0,1.0,0.0,0.0,0.0 172 | 170,-0.31884423,0.0,0.0,1.0,0.0,0.0,0.0 173 | 171,-0.32336682,0.0,0.0,1.0,0.0,0.0,0.0 174 | 172,-0.32788944,0.0,0.0,1.0,0.0,0.0,0.0 175 | 173,-0.33241206,0.0,0.0,1.0,0.0,0.0,0.0 176 | 174,-0.33693466,0.0,0.0,1.0,0.0,0.0,0.0 177 | 175,-0.34145728,0.0,0.0,1.0,0.0,0.0,0.0 178 | 176,-0.3459799,0.0,0.0,1.0,0.0,0.0,0.0 179 | 177,-0.35050252,0.0,0.0,1.0,0.0,0.0,0.0 180 | 178,-0.3550251,0.0,0.0,1.0,0.0,0.0,0.0 181 | 179,-0.35954773,0.0,0.0,1.0,0.0,0.0,0.0 182 | 180,-0.36407036,0.0,0.0,1.0,0.0,0.0,0.0 183 | 181,-0.36859295,0.0,0.0,1.0,0.0,0.0,0.0 184 | 182,-0.37311557,0.0,0.0,1.0,0.0,0.0,0.0 185 | 183,-0.3776382,0.0,0.0,1.0,0.0,0.0,0.0 186 | 184,-0.38216078,0.0,0.0,1.0,0.0,0.0,0.0 187 | 185,-0.3866834,0.0,0.0,1.0,0.0,0.0,0.0 188 | 186,-0.39120603,0.0,0.0,1.0,0.0,0.0,0.0 189 | 187,-0.39572865,0.0,0.0,1.0,0.0,0.0,0.0 190 | 188,-0.40025124,0.0,0.0,1.0,0.0,0.0,0.0 191 | 189,-0.40477386,0.0,0.0,1.0,0.0,0.0,0.0 192 | 190,-0.40929648,0.0,0.0,1.0,0.0,0.0,0.0 193 | 191,-0.41381907,0.0,0.0,1.0,0.0,0.0,0.0 194 | 192,-0.4183417,0.0,0.0,1.0,0.0,0.0,0.0 195 | 193,-0.42286432,0.0,0.0,1.0,0.0,0.0,0.0 196 | 194,-0.4273869,0.0,0.0,1.0,0.0,0.0,0.0 197 | 195,-0.43190953,0.0,0.0,1.0,0.0,0.0,0.0 198 | 196,-0.43643215,0.0,0.0,1.0,0.0,0.0,0.0 199 | 197,-0.44095477,0.0,0.0,1.0,0.0,0.0,0.0 200 | 198,-0.44547737,0.0,0.0,1.0,0.0,0.0,0.0 201 | 199,-0.45,0.0,0.0,1.0,0.0,0.0,0.0 -------------------------------------------------------------------------------- /cppflow/paths/README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | Paths are from the 'TORM' github repo: https://github.com/cheulkang/TORM/tree/main/src/data/scene 4 | 5 | These paths (I believe) use a format `1.0/0.0;x,y,z;qw,qx,qy,qz`, where the `1.0/0.0` indicates that the first value is either a 1 or a 0. I don't know what this value represents. 6 | 7 | > Note: I'm pretty sure the quaternions in the torm paths are in `w,x,y,z` format. The quaternions are created like `tf::Quaternion q(input_rots[i][1], input_rots[i][2], input_rots[i][3], input_rots[i][0])` (see https://github.com/cheulkang/TORM/blob/main/src/torm_problem.cpp#L122). The definition for `tf::Quaternion` is `Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w)` (defined here https://github.com/ros/geometry/blob/noetic-devel/tf/include/tf/LinearMath/Quaternion.h#L39). This indicates the quaternions in paths_torm/ are saved in `w,x,y,z` format. 8 | 9 | I have updated these path files to .csv files, with the format `x,y,z,qw,qx,wy,wz` -------------------------------------------------------------------------------- /cppflow/paths/flappy_bird.csv: -------------------------------------------------------------------------------- 1 | ,0,1,2,3,4,5,6 2 | 0,0.45,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 3 | 1,0.44547737,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 4 | 2,0.44095477,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 5 | 3,0.43643215,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 6 | 4,0.43190953,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 7 | 5,0.4273869,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 8 | 6,0.42286432,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 9 | 7,0.4183417,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 10 | 8,0.41381907,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 11 | 9,0.40929648,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 12 | 10,0.40477386,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 13 | 11,0.40025124,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 14 | 12,0.39572865,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 15 | 13,0.39120603,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 16 | 14,0.3866834,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 17 | 15,0.38216078,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 18 | 16,0.3776382,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 19 | 17,0.37311557,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 20 | 18,0.36859295,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 21 | 19,0.36407036,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 22 | 20,0.35954773,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 23 | 21,0.3550251,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 24 | 22,0.35050252,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 25 | 23,0.3459799,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 26 | 24,0.34145728,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 27 | 25,0.33693466,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 28 | 26,0.33241206,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 29 | 27,0.32788944,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 30 | 28,0.32336682,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 31 | 29,0.31884423,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 32 | 30,0.3143216,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 33 | 31,0.309799,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 34 | 32,0.3052764,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 35 | 33,0.30075377,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 36 | 34,0.29623115,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 37 | 35,0.29170853,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 38 | 36,0.28718594,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 39 | 37,0.28266332,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 40 | 38,0.2781407,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 41 | 39,0.2736181,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 42 | 40,0.26909548,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 43 | 41,0.26457286,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 44 | 42,0.26005024,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 45 | 43,0.25552765,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 46 | 44,0.25100502,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 47 | 45,0.24648242,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 48 | 46,0.2419598,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 49 | 47,0.23743719,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 50 | 48,0.23291457,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 51 | 49,0.22839196,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 52 | 50,0.22386935,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 53 | 51,0.21934673,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 54 | 52,0.21482413,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 55 | 53,0.2103015,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 56 | 54,0.2057789,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 57 | 55,0.20125629,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 58 | 56,0.19673367,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 59 | 57,0.19221106,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 60 | 58,0.18768844,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 61 | 59,0.18316583,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 62 | 60,0.17864323,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 63 | 61,0.1741206,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 64 | 62,0.169598,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 65 | 63,0.16507538,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 66 | 64,0.16055277,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 67 | 65,0.15603016,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 68 | 66,0.15150754,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 69 | 67,0.14698493,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 70 | 68,0.14246231,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 71 | 69,0.1379397,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 72 | 70,0.1334171,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 73 | 71,0.12889448,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 74 | 72,0.12437187,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 75 | 73,0.11984926,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 76 | 74,0.11532664,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 77 | 75,0.11080403,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 78 | 76,0.106281415,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 79 | 77,0.1017588,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 80 | 78,0.09723619,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 81 | 79,0.09271358,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 82 | 80,0.088190965,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 83 | 81,0.08366835,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 84 | 82,0.07914574,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 85 | 83,0.07462313,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 86 | 84,0.070100516,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 87 | 85,0.0655779,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 88 | 86,0.061055288,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 89 | 87,0.056532677,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 90 | 88,0.052010063,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 91 | 89,0.04748745,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 92 | 90,0.04296484,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 93 | 91,0.038442224,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 94 | 92,0.03391961,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 95 | 93,0.029397,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 96 | 94,0.024874385,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 97 | 95,0.020351773,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 98 | 96,0.01582916,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 99 | 97,0.0113065485,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 100 | 98,0.0067839352,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 101 | 99,0.0022613225,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 102 | 100,-0.0022613225,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 103 | 101,-0.0067839352,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 104 | 102,-0.0113065485,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 105 | 103,-0.01582916,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 106 | 104,-0.020351773,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 107 | 105,-0.024874385,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 108 | 106,-0.029397,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 109 | 107,-0.03391961,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 110 | 108,-0.038442224,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 111 | 109,-0.04296484,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 112 | 110,-0.04748745,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 113 | 111,-0.052010063,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 114 | 112,-0.056532677,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 115 | 113,-0.061055288,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 116 | 114,-0.0655779,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 117 | 115,-0.070100516,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 118 | 116,-0.07462313,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 119 | 117,-0.07914574,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 120 | 118,-0.08366835,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 121 | 119,-0.088190965,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 122 | 120,-0.09271358,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 123 | 121,-0.09723619,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 124 | 122,-0.1017588,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 125 | 123,-0.106281415,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 126 | 124,-0.11080403,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 127 | 125,-0.11532664,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 128 | 126,-0.11984926,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 129 | 127,-0.12437187,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 130 | 128,-0.12889448,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 131 | 129,-0.1334171,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 132 | 130,-0.1379397,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 133 | 131,-0.14246231,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 134 | 132,-0.14698493,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 135 | 133,-0.15150754,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 136 | 134,-0.15603016,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 137 | 135,-0.16055277,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 138 | 136,-0.16507538,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 139 | 137,-0.169598,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 140 | 138,-0.1741206,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 141 | 139,-0.17864323,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 142 | 140,-0.18316583,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 143 | 141,-0.18768844,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 144 | 142,-0.19221106,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 145 | 143,-0.19673367,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 146 | 144,-0.20125629,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 147 | 145,-0.2057789,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 148 | 146,-0.2103015,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 149 | 147,-0.21482413,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 150 | 148,-0.21934673,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 151 | 149,-0.22386935,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 152 | 150,-0.22839196,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 153 | 151,-0.23291457,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 154 | 152,-0.23743719,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 155 | 153,-0.2419598,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 156 | 154,-0.24648242,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 157 | 155,-0.25100502,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 158 | 156,-0.25552765,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 159 | 157,-0.26005024,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 160 | 158,-0.26457286,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 161 | 159,-0.26909548,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 162 | 160,-0.2736181,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 163 | 161,-0.2781407,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 164 | 162,-0.28266332,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 165 | 163,-0.28718594,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 166 | 164,-0.29170853,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 167 | 165,-0.29623115,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 168 | 166,-0.30075377,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 169 | 167,-0.3052764,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 170 | 168,-0.309799,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 171 | 169,-0.3143216,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 172 | 170,-0.31884423,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 173 | 171,-0.32336682,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 174 | 172,-0.32788944,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 175 | 173,-0.33241206,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 176 | 174,-0.33693466,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 177 | 175,-0.34145728,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 178 | 176,-0.3459799,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 179 | 177,-0.35050252,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 180 | 178,-0.3550251,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 181 | 179,-0.35954773,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 182 | 180,-0.36407036,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 183 | 181,-0.36859295,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 184 | 182,-0.37311557,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 185 | 183,-0.3776382,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 186 | 184,-0.38216078,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 187 | 185,-0.3866834,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 188 | 186,-0.39120603,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 189 | 187,-0.39572865,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 190 | 188,-0.40025124,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 191 | 189,-0.40477386,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 192 | 190,-0.40929648,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 193 | 191,-0.41381907,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 194 | 192,-0.4183417,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 195 | 193,-0.42286432,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 196 | 194,-0.4273869,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 197 | 195,-0.43190953,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 198 | 196,-0.43643215,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 199 | 197,-0.44095477,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 200 | 198,-0.44547737,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 201 | 199,-0.45,0.0,0.0,0.7057944,-0.70841676,0.0,0.0 202 | -------------------------------------------------------------------------------- /cppflow/paths/hello_mini.csv: -------------------------------------------------------------------------------- 1 | time,x,y,z,qw,qx,qy,qz 2 | 0.0,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000 3 | 0.03623188,0.000000,-0.001431,0.003984,1.000000,0.000000,0.000000,0.000000 4 | 0.07246377,0.000000,-0.003492,0.008373,1.000000,0.000000,0.000000,0.000000 5 | 0.10869565,0.000000,-0.006160,0.013158,1.000000,0.000000,0.000000,0.000000 6 | 0.14492754,0.000000,-0.009410,0.018331,1.000000,0.000000,0.000000,0.000000 7 | 0.18115942,0.000000,-0.013217,0.023884,1.000000,0.000000,0.000000,0.000000 8 | 0.2173913,0.000000,-0.017557,0.029808,1.000000,0.000000,0.000000,0.000000 9 | 0.25362319,0.000000,-0.022404,0.036095,1.000000,0.000000,0.000000,0.000000 10 | 0.28985507,0.000000,-0.027736,0.042737,1.000000,0.000000,0.000000,0.000000 11 | 0.32608696,0.000000,-0.033526,0.049726,1.000000,0.000000,0.000000,0.000000 12 | 0.36231884,0.000000,-0.039751,0.057052,1.000000,0.000000,0.000000,0.000000 13 | 0.39855072,0.000000,-0.046386,0.064708,1.000000,0.000000,0.000000,0.000000 14 | 0.43478261,0.000000,-0.053405,0.072686,1.000000,0.000000,0.000000,0.000000 15 | 0.47101449,0.000000,-0.060786,0.080977,1.000000,0.000000,0.000000,0.000000 16 | 0.50724638,0.000000,-0.068503,0.089573,1.000000,0.000000,0.000000,0.000000 17 | 0.54347826,0.000000,-0.076531,0.098465,1.000000,0.000000,0.000000,0.000000 18 | 0.57971014,0.000000,-0.084847,0.107646,1.000000,0.000000,0.000000,0.000000 19 | 0.61594203,0.000000,-0.093424,0.117106,1.000000,0.000000,0.000000,0.000000 20 | 0.65217391,0.000000,-0.102240,0.126838,1.000000,0.000000,0.000000,0.000000 21 | 0.6884058,0.000000,-0.111269,0.136833,1.000000,0.000000,0.000000,0.000000 22 | 0.72463768,0.000000,-0.120487,0.147084,1.000000,0.000000,0.000000,0.000000 23 | 0.76086957,0.000000,-0.129869,0.157581,1.000000,0.000000,0.000000,0.000000 24 | 0.79710145,0.000000,-0.139391,0.168316,1.000000,0.000000,0.000000,0.000000 25 | 0.83333333,0.000000,-0.149027,0.179281,1.000000,0.000000,0.000000,0.000000 26 | 0.86956522,0.000000,-0.158755,0.190468,1.000000,0.000000,0.000000,0.000000 -------------------------------------------------------------------------------- /cppflow/paths/paths_torm/rot_yz: -------------------------------------------------------------------------------- 1 | 1.00;0.000000,0.000000,0.000000;1.000000,0.000000,0.000000,0.000000 2 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,0.015103,0.000000 3 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,0.030203,0.000000 4 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,0.045296,0.000000 5 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,0.060378,0.000000 6 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,0.075447,0.000000 7 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,0.090499,0.000000 8 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,0.105530,0.000000 9 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,0.120537,0.000000 10 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,0.135516,0.000000 11 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,0.150465,0.000000 12 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,0.165379,0.000000 13 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,0.180255,0.000000 14 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,0.195090,0.000000 15 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,0.209881,0.000000 16 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,0.224624,0.000000 17 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,0.239316,0.000000 18 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,0.253953,0.000000 19 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,0.268532,0.000000 20 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,0.283050,0.000000 21 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,0.297503,0.000000 22 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,0.311888,0.000000 23 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,0.326203,0.000000 24 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,0.340443,0.000000 25 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,0.354605,0.000000 26 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,0.368686,0.000000 27 | 1.00;0.000000,0.000000,0.000000;0.923880,0.000000,0.382683,0.000000 28 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,0.368686,0.000000 29 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,0.354605,0.000000 30 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,0.340443,0.000000 31 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,0.326203,0.000000 32 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,0.311888,0.000000 33 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,0.297503,0.000000 34 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,0.283050,0.000000 35 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,0.268532,0.000000 36 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,0.253953,0.000000 37 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,0.239316,0.000000 38 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,0.224624,0.000000 39 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,0.209881,0.000000 40 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,0.195090,0.000000 41 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,0.180255,0.000000 42 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,0.165379,0.000000 43 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,0.150465,0.000000 44 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,0.135516,0.000000 45 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,0.120537,0.000000 46 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,0.105530,0.000000 47 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,0.090499,0.000000 48 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,0.075447,0.000000 49 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,0.060378,0.000000 50 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,0.045296,0.000000 51 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,0.030203,0.000000 52 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,0.015103,0.000000 53 | 1.00;0.000000,0.000000,0.000000;1.000000,0.000000,0.000000,0.000000 54 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,-0.015103,0.000000 55 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,-0.030203,0.000000 56 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,-0.045296,0.000000 57 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,-0.060378,0.000000 58 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,-0.075447,0.000000 59 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,-0.090499,0.000000 60 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,-0.105530,0.000000 61 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,-0.120537,0.000000 62 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,-0.135516,0.000000 63 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,-0.150465,0.000000 64 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,-0.165379,0.000000 65 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,-0.180255,0.000000 66 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,-0.195090,0.000000 67 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,-0.209881,0.000000 68 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,-0.224624,0.000000 69 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,-0.239316,0.000000 70 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,-0.253953,0.000000 71 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,-0.268532,0.000000 72 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,-0.283050,0.000000 73 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,-0.297503,0.000000 74 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,-0.311888,0.000000 75 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,-0.326203,0.000000 76 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,-0.340443,0.000000 77 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,-0.354605,0.000000 78 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,-0.368686,0.000000 79 | 1.00;0.000000,0.000000,0.000000;0.923880,0.000000,-0.382683,0.000000 80 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,-0.368686,0.000000 81 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,-0.354605,0.000000 82 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,-0.340443,0.000000 83 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,-0.326203,0.000000 84 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,-0.311888,0.000000 85 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,-0.297503,0.000000 86 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,-0.283050,0.000000 87 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,-0.268532,0.000000 88 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,-0.253953,0.000000 89 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,-0.239316,0.000000 90 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,-0.224624,0.000000 91 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,-0.209881,0.000000 92 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,-0.195090,0.000000 93 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,-0.180255,0.000000 94 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,-0.165379,0.000000 95 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,-0.150465,0.000000 96 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,-0.135516,0.000000 97 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,-0.120537,0.000000 98 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,-0.105530,0.000000 99 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,-0.090499,0.000000 100 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,-0.075447,0.000000 101 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,-0.060378,0.000000 102 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,-0.045296,0.000000 103 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,-0.030203,0.000000 104 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,-0.015103,0.000000 105 | 1.00;0.000000,0.000000,0.000000;1.000000,0.000000,0.000000,0.000000 106 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,0.000000,0.015103 107 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,0.000000,0.030203 108 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,0.000000,0.045296 109 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,0.000000,0.060378 110 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,0.000000,0.075447 111 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,0.000000,0.090499 112 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,0.000000,0.105530 113 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,0.000000,0.120537 114 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,0.000000,0.135516 115 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,0.000000,0.150465 116 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,0.000000,0.165379 117 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,0.000000,0.180255 118 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,0.000000,0.195090 119 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,0.000000,0.209881 120 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,0.000000,0.224624 121 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,0.000000,0.239316 122 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,0.000000,0.253953 123 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,0.000000,0.268532 124 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,0.000000,0.283050 125 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,0.000000,0.297503 126 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,0.000000,0.311888 127 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,0.000000,0.326203 128 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,0.000000,0.340443 129 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,0.000000,0.354605 130 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,0.000000,0.368686 131 | 1.00;0.000000,0.000000,0.000000;0.923880,0.000000,0.000000,0.382683 132 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,0.000000,0.368686 133 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,0.000000,0.354605 134 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,0.000000,0.340443 135 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,0.000000,0.326203 136 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,0.000000,0.311888 137 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,0.000000,0.297503 138 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,0.000000,0.283050 139 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,0.000000,0.268532 140 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,0.000000,0.253953 141 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,0.000000,0.239316 142 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,0.000000,0.224624 143 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,0.000000,0.209881 144 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,0.000000,0.195090 145 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,0.000000,0.180255 146 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,0.000000,0.165379 147 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,0.000000,0.150465 148 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,0.000000,0.135516 149 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,0.000000,0.120537 150 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,0.000000,0.105530 151 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,0.000000,0.090499 152 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,0.000000,0.075447 153 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,0.000000,0.060378 154 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,0.000000,0.045296 155 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,0.000000,0.030203 156 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,0.000000,0.015103 157 | 1.00;0.000000,0.000000,0.000000;1.000000,0.000000,0.000000,0.000000 158 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,0.000000,-0.015103 159 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,0.000000,-0.030203 160 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,0.000000,-0.045296 161 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,0.000000,-0.060378 162 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,0.000000,-0.075447 163 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,0.000000,-0.090499 164 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,0.000000,-0.105530 165 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,0.000000,-0.120537 166 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,0.000000,-0.135516 167 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,0.000000,-0.150465 168 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,0.000000,-0.165379 169 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,0.000000,-0.180255 170 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,0.000000,-0.195090 171 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,0.000000,-0.209881 172 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,0.000000,-0.224624 173 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,0.000000,-0.239316 174 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,0.000000,-0.253953 175 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,0.000000,-0.268532 176 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,0.000000,-0.283050 177 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,0.000000,-0.297503 178 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,0.000000,-0.311888 179 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,0.000000,-0.326203 180 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,0.000000,-0.340443 181 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,0.000000,-0.354605 182 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,0.000000,-0.368686 183 | 1.00;0.000000,0.000000,0.000000;0.923880,0.000000,0.000000,-0.382683 184 | 0.00;0.000000,0.000000,0.000000;0.929554,0.000000,0.000000,-0.368686 185 | 0.00;0.000000,0.000000,0.000000;0.935016,0.000000,0.000000,-0.354605 186 | 0.00;0.000000,0.000000,0.000000;0.940265,0.000000,0.000000,-0.340443 187 | 0.00;0.000000,0.000000,0.000000;0.945300,0.000000,0.000000,-0.326203 188 | 0.00;0.000000,0.000000,0.000000;0.950119,0.000000,0.000000,-0.311888 189 | 0.00;0.000000,0.000000,0.000000;0.954721,0.000000,0.000000,-0.297503 190 | 0.00;0.000000,0.000000,0.000000;0.959105,0.000000,0.000000,-0.283050 191 | 0.00;0.000000,0.000000,0.000000;0.963271,0.000000,0.000000,-0.268532 192 | 0.00;0.000000,0.000000,0.000000;0.967217,0.000000,0.000000,-0.253953 193 | 0.00;0.000000,0.000000,0.000000;0.970942,0.000000,0.000000,-0.239316 194 | 0.00;0.000000,0.000000,0.000000;0.974446,0.000000,0.000000,-0.224624 195 | 0.00;0.000000,0.000000,0.000000;0.977727,0.000000,0.000000,-0.209881 196 | 0.00;0.000000,0.000000,0.000000;0.980785,0.000000,0.000000,-0.195090 197 | 0.00;0.000000,0.000000,0.000000;0.983620,0.000000,0.000000,-0.180255 198 | 0.00;0.000000,0.000000,0.000000;0.986230,0.000000,0.000000,-0.165379 199 | 0.00;0.000000,0.000000,0.000000;0.988615,0.000000,0.000000,-0.150465 200 | 0.00;0.000000,0.000000,0.000000;0.990775,0.000000,0.000000,-0.135516 201 | 0.00;0.000000,0.000000,0.000000;0.992709,0.000000,0.000000,-0.120537 202 | 0.00;0.000000,0.000000,0.000000;0.994416,0.000000,0.000000,-0.105530 203 | 0.00;0.000000,0.000000,0.000000;0.995897,0.000000,0.000000,-0.090499 204 | 0.00;0.000000,0.000000,0.000000;0.997150,0.000000,0.000000,-0.075447 205 | 0.00;0.000000,0.000000,0.000000;0.998176,0.000000,0.000000,-0.060378 206 | 0.00;0.000000,0.000000,0.000000;0.998974,0.000000,0.000000,-0.045296 207 | 0.00;0.000000,0.000000,0.000000;0.999544,0.000000,0.000000,-0.030203 208 | 0.00;0.000000,0.000000,0.000000;0.999886,0.000000,0.000000,-0.015103 209 | 1.00;0.000000,0.000000,0.000000;1.000000,0.000000,0.000000,0.000000 210 | -------------------------------------------------------------------------------- /cppflow/paths/rot_yz.csv: -------------------------------------------------------------------------------- 1 | time,x,y,z,qw,qx,qy,qz 2 | 0.0,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000 3 | 0.09615385,0.000000,0.000000,0.000000,0.999886,0.000000,0.015103,0.000000 4 | 0.19230769,0.000000,0.000000,0.000000,0.999544,0.000000,0.030203,0.000000 5 | 0.28846154,0.000000,0.000000,0.000000,0.998974,0.000000,0.045296,0.000000 6 | 0.38461538,0.000000,0.000000,0.000000,0.998176,0.000000,0.060378,0.000000 7 | 0.48076923,0.000000,0.000000,0.000000,0.997150,0.000000,0.075447,0.000000 8 | 0.57692308,0.000000,0.000000,0.000000,0.995897,0.000000,0.090499,0.000000 9 | 0.67307692,0.000000,0.000000,0.000000,0.994416,0.000000,0.105530,0.000000 10 | 0.76923077,0.000000,0.000000,0.000000,0.992709,0.000000,0.120537,0.000000 11 | 0.86538462,0.000000,0.000000,0.000000,0.990775,0.000000,0.135516,0.000000 12 | 0.96153846,0.000000,0.000000,0.000000,0.988615,0.000000,0.150465,0.000000 13 | 1.05769231,0.000000,0.000000,0.000000,0.986230,0.000000,0.165379,0.000000 14 | 1.15384615,0.000000,0.000000,0.000000,0.983620,0.000000,0.180255,0.000000 15 | 1.25,0.000000,0.000000,0.000000,0.980785,0.000000,0.195090,0.000000 16 | 1.34615385,0.000000,0.000000,0.000000,0.977727,0.000000,0.209881,0.000000 17 | 1.44230769,0.000000,0.000000,0.000000,0.974446,0.000000,0.224624,0.000000 18 | 1.53846154,0.000000,0.000000,0.000000,0.970942,0.000000,0.239316,0.000000 19 | 1.63461538,0.000000,0.000000,0.000000,0.967217,0.000000,0.253953,0.000000 20 | 1.73076923,0.000000,0.000000,0.000000,0.963271,0.000000,0.268532,0.000000 21 | 1.82692308,0.000000,0.000000,0.000000,0.959105,0.000000,0.283050,0.000000 22 | 1.92307692,0.000000,0.000000,0.000000,0.954721,0.000000,0.297503,0.000000 23 | 2.01923077,0.000000,0.000000,0.000000,0.950119,0.000000,0.311888,0.000000 24 | 2.11538462,0.000000,0.000000,0.000000,0.945300,0.000000,0.326203,0.000000 25 | 2.21153846,0.000000,0.000000,0.000000,0.940265,0.000000,0.340443,0.000000 26 | 2.30769231,0.000000,0.000000,0.000000,0.935016,0.000000,0.354605,0.000000 27 | 2.40384615,0.000000,0.000000,0.000000,0.929554,0.000000,0.368686,0.000000 28 | 2.5,0.000000,0.000000,0.000000,0.923880,0.000000,0.382683,0.000000 29 | 2.59615385,0.000000,0.000000,0.000000,0.929554,0.000000,0.368686,0.000000 30 | 2.69230769,0.000000,0.000000,0.000000,0.935016,0.000000,0.354605,0.000000 31 | 2.78846154,0.000000,0.000000,0.000000,0.940265,0.000000,0.340443,0.000000 32 | 2.88461538,0.000000,0.000000,0.000000,0.945300,0.000000,0.326203,0.000000 33 | 2.98076923,0.000000,0.000000,0.000000,0.950119,0.000000,0.311888,0.000000 34 | 3.07692308,0.000000,0.000000,0.000000,0.954721,0.000000,0.297503,0.000000 35 | 3.17307692,0.000000,0.000000,0.000000,0.959105,0.000000,0.283050,0.000000 36 | 3.26923077,0.000000,0.000000,0.000000,0.963271,0.000000,0.268532,0.000000 37 | 3.36538462,0.000000,0.000000,0.000000,0.967217,0.000000,0.253953,0.000000 38 | 3.46153846,0.000000,0.000000,0.000000,0.970942,0.000000,0.239316,0.000000 39 | 3.55769231,0.000000,0.000000,0.000000,0.974446,0.000000,0.224624,0.000000 40 | 3.65384615,0.000000,0.000000,0.000000,0.977727,0.000000,0.209881,0.000000 41 | 3.75,0.000000,0.000000,0.000000,0.980785,0.000000,0.195090,0.000000 42 | 3.84615385,0.000000,0.000000,0.000000,0.983620,0.000000,0.180255,0.000000 43 | 3.94230769,0.000000,0.000000,0.000000,0.986230,0.000000,0.165379,0.000000 44 | 4.03846154,0.000000,0.000000,0.000000,0.988615,0.000000,0.150465,0.000000 45 | 4.13461538,0.000000,0.000000,0.000000,0.990775,0.000000,0.135516,0.000000 46 | 4.23076923,0.000000,0.000000,0.000000,0.992709,0.000000,0.120537,0.000000 47 | 4.32692308,0.000000,0.000000,0.000000,0.994416,0.000000,0.105530,0.000000 48 | 4.42307692,0.000000,0.000000,0.000000,0.995897,0.000000,0.090499,0.000000 49 | 4.51923077,0.000000,0.000000,0.000000,0.997150,0.000000,0.075447,0.000000 50 | 4.61538462,0.000000,0.000000,0.000000,0.998176,0.000000,0.060378,0.000000 51 | 4.71153846,0.000000,0.000000,0.000000,0.998974,0.000000,0.045296,0.000000 52 | 4.80769231,0.000000,0.000000,0.000000,0.999544,0.000000,0.030203,0.000000 53 | 4.90384615,0.000000,0.000000,0.000000,0.999886,0.000000,0.015103,0.000000 54 | 5.0,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000 55 | 5.09615385,0.000000,0.000000,0.000000,0.999886,0.000000,-0.015103,0.000000 56 | 5.19230769,0.000000,0.000000,0.000000,0.999544,0.000000,-0.030203,0.000000 57 | 5.28846154,0.000000,0.000000,0.000000,0.998974,0.000000,-0.045296,0.000000 58 | 5.38461538,0.000000,0.000000,0.000000,0.998176,0.000000,-0.060378,0.000000 59 | 5.48076923,0.000000,0.000000,0.000000,0.997150,0.000000,-0.075447,0.000000 60 | 5.57692308,0.000000,0.000000,0.000000,0.995897,0.000000,-0.090499,0.000000 61 | 5.67307692,0.000000,0.000000,0.000000,0.994416,0.000000,-0.105530,0.000000 62 | 5.76923077,0.000000,0.000000,0.000000,0.992709,0.000000,-0.120537,0.000000 63 | 5.86538462,0.000000,0.000000,0.000000,0.990775,0.000000,-0.135516,0.000000 64 | 5.96153846,0.000000,0.000000,0.000000,0.988615,0.000000,-0.150465,0.000000 65 | 6.05769231,0.000000,0.000000,0.000000,0.986230,0.000000,-0.165379,0.000000 66 | 6.15384615,0.000000,0.000000,0.000000,0.983620,0.000000,-0.180255,0.000000 67 | 6.25,0.000000,0.000000,0.000000,0.980785,0.000000,-0.195090,0.000000 68 | 6.34615385,0.000000,0.000000,0.000000,0.977727,0.000000,-0.209881,0.000000 69 | 6.44230769,0.000000,0.000000,0.000000,0.974446,0.000000,-0.224624,0.000000 70 | 6.53846154,0.000000,0.000000,0.000000,0.970942,0.000000,-0.239316,0.000000 71 | 6.63461538,0.000000,0.000000,0.000000,0.967217,0.000000,-0.253953,0.000000 72 | 6.73076923,0.000000,0.000000,0.000000,0.963271,0.000000,-0.268532,0.000000 73 | 6.82692308,0.000000,0.000000,0.000000,0.959105,0.000000,-0.283050,0.000000 74 | 6.92307692,0.000000,0.000000,0.000000,0.954721,0.000000,-0.297503,0.000000 75 | 7.01923077,0.000000,0.000000,0.000000,0.950119,0.000000,-0.311888,0.000000 76 | 7.11538462,0.000000,0.000000,0.000000,0.945300,0.000000,-0.326203,0.000000 77 | 7.21153846,0.000000,0.000000,0.000000,0.940265,0.000000,-0.340443,0.000000 78 | 7.30769231,0.000000,0.000000,0.000000,0.935016,0.000000,-0.354605,0.000000 79 | 7.40384615,0.000000,0.000000,0.000000,0.929554,0.000000,-0.368686,0.000000 80 | 7.5,0.000000,0.000000,0.000000,0.923880,0.000000,-0.382683,0.000000 81 | 7.59615385,0.000000,0.000000,0.000000,0.929554,0.000000,-0.368686,0.000000 82 | 7.69230769,0.000000,0.000000,0.000000,0.935016,0.000000,-0.354605,0.000000 83 | 7.78846154,0.000000,0.000000,0.000000,0.940265,0.000000,-0.340443,0.000000 84 | 7.88461538,0.000000,0.000000,0.000000,0.945300,0.000000,-0.326203,0.000000 85 | 7.98076923,0.000000,0.000000,0.000000,0.950119,0.000000,-0.311888,0.000000 86 | 8.07692308,0.000000,0.000000,0.000000,0.954721,0.000000,-0.297503,0.000000 87 | 8.17307692,0.000000,0.000000,0.000000,0.959105,0.000000,-0.283050,0.000000 88 | 8.26923077,0.000000,0.000000,0.000000,0.963271,0.000000,-0.268532,0.000000 89 | 8.36538462,0.000000,0.000000,0.000000,0.967217,0.000000,-0.253953,0.000000 90 | 8.46153846,0.000000,0.000000,0.000000,0.970942,0.000000,-0.239316,0.000000 91 | 8.55769231,0.000000,0.000000,0.000000,0.974446,0.000000,-0.224624,0.000000 92 | 8.65384615,0.000000,0.000000,0.000000,0.977727,0.000000,-0.209881,0.000000 93 | 8.75,0.000000,0.000000,0.000000,0.980785,0.000000,-0.195090,0.000000 94 | 8.84615385,0.000000,0.000000,0.000000,0.983620,0.000000,-0.180255,0.000000 95 | 8.94230769,0.000000,0.000000,0.000000,0.986230,0.000000,-0.165379,0.000000 96 | 9.03846154,0.000000,0.000000,0.000000,0.988615,0.000000,-0.150465,0.000000 97 | 9.13461538,0.000000,0.000000,0.000000,0.990775,0.000000,-0.135516,0.000000 98 | 9.23076923,0.000000,0.000000,0.000000,0.992709,0.000000,-0.120537,0.000000 99 | 9.32692308,0.000000,0.000000,0.000000,0.994416,0.000000,-0.105530,0.000000 100 | 9.42307692,0.000000,0.000000,0.000000,0.995897,0.000000,-0.090499,0.000000 101 | 9.51923077,0.000000,0.000000,0.000000,0.997150,0.000000,-0.075447,0.000000 102 | 9.61538462,0.000000,0.000000,0.000000,0.998176,0.000000,-0.060378,0.000000 103 | 9.71153846,0.000000,0.000000,0.000000,0.998974,0.000000,-0.045296,0.000000 104 | 9.80769231,0.000000,0.000000,0.000000,0.999544,0.000000,-0.030203,0.000000 105 | 9.90384615,0.000000,0.000000,0.000000,0.999886,0.000000,-0.015103,0.000000 106 | 10.0,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000 107 | 10.09615385,0.000000,0.000000,0.000000,0.999886,0.000000,0.000000,0.015103 108 | 10.19230769,0.000000,0.000000,0.000000,0.999544,0.000000,0.000000,0.030203 109 | 10.28846154,0.000000,0.000000,0.000000,0.998974,0.000000,0.000000,0.045296 110 | 10.38461538,0.000000,0.000000,0.000000,0.998176,0.000000,0.000000,0.060378 111 | 10.48076923,0.000000,0.000000,0.000000,0.997150,0.000000,0.000000,0.075447 112 | 10.57692308,0.000000,0.000000,0.000000,0.995897,0.000000,0.000000,0.090499 113 | 10.67307692,0.000000,0.000000,0.000000,0.994416,0.000000,0.000000,0.105530 114 | 10.76923077,0.000000,0.000000,0.000000,0.992709,0.000000,0.000000,0.120537 115 | 10.86538462,0.000000,0.000000,0.000000,0.990775,0.000000,0.000000,0.135516 116 | 10.96153846,0.000000,0.000000,0.000000,0.988615,0.000000,0.000000,0.150465 117 | 11.05769231,0.000000,0.000000,0.000000,0.986230,0.000000,0.000000,0.165379 118 | 11.15384615,0.000000,0.000000,0.000000,0.983620,0.000000,0.000000,0.180255 119 | 11.25,0.000000,0.000000,0.000000,0.980785,0.000000,0.000000,0.195090 120 | 11.34615385,0.000000,0.000000,0.000000,0.977727,0.000000,0.000000,0.209881 121 | 11.44230769,0.000000,0.000000,0.000000,0.974446,0.000000,0.000000,0.224624 122 | 11.53846154,0.000000,0.000000,0.000000,0.970942,0.000000,0.000000,0.239316 123 | 11.63461538,0.000000,0.000000,0.000000,0.967217,0.000000,0.000000,0.253953 124 | 11.73076923,0.000000,0.000000,0.000000,0.963271,0.000000,0.000000,0.268532 125 | 11.82692308,0.000000,0.000000,0.000000,0.959105,0.000000,0.000000,0.283050 126 | 11.92307692,0.000000,0.000000,0.000000,0.954721,0.000000,0.000000,0.297503 127 | 12.01923077,0.000000,0.000000,0.000000,0.950119,0.000000,0.000000,0.311888 128 | 12.11538462,0.000000,0.000000,0.000000,0.945300,0.000000,0.000000,0.326203 129 | 12.21153846,0.000000,0.000000,0.000000,0.940265,0.000000,0.000000,0.340443 130 | 12.30769231,0.000000,0.000000,0.000000,0.935016,0.000000,0.000000,0.354605 131 | 12.40384615,0.000000,0.000000,0.000000,0.929554,0.000000,0.000000,0.368686 132 | 12.5,0.000000,0.000000,0.000000,0.923880,0.000000,0.000000,0.382683 133 | 12.59615385,0.000000,0.000000,0.000000,0.929554,0.000000,0.000000,0.368686 134 | 12.69230769,0.000000,0.000000,0.000000,0.935016,0.000000,0.000000,0.354605 135 | 12.78846154,0.000000,0.000000,0.000000,0.940265,0.000000,0.000000,0.340443 136 | 12.88461538,0.000000,0.000000,0.000000,0.945300,0.000000,0.000000,0.326203 137 | 12.98076923,0.000000,0.000000,0.000000,0.950119,0.000000,0.000000,0.311888 138 | 13.07692308,0.000000,0.000000,0.000000,0.954721,0.000000,0.000000,0.297503 139 | 13.17307692,0.000000,0.000000,0.000000,0.959105,0.000000,0.000000,0.283050 140 | 13.26923077,0.000000,0.000000,0.000000,0.963271,0.000000,0.000000,0.268532 141 | 13.36538462,0.000000,0.000000,0.000000,0.967217,0.000000,0.000000,0.253953 142 | 13.46153846,0.000000,0.000000,0.000000,0.970942,0.000000,0.000000,0.239316 143 | 13.55769231,0.000000,0.000000,0.000000,0.974446,0.000000,0.000000,0.224624 144 | 13.65384615,0.000000,0.000000,0.000000,0.977727,0.000000,0.000000,0.209881 145 | 13.75,0.000000,0.000000,0.000000,0.980785,0.000000,0.000000,0.195090 146 | 13.84615385,0.000000,0.000000,0.000000,0.983620,0.000000,0.000000,0.180255 147 | 13.94230769,0.000000,0.000000,0.000000,0.986230,0.000000,0.000000,0.165379 148 | 14.03846154,0.000000,0.000000,0.000000,0.988615,0.000000,0.000000,0.150465 149 | 14.13461538,0.000000,0.000000,0.000000,0.990775,0.000000,0.000000,0.135516 150 | 14.23076923,0.000000,0.000000,0.000000,0.992709,0.000000,0.000000,0.120537 151 | 14.32692308,0.000000,0.000000,0.000000,0.994416,0.000000,0.000000,0.105530 152 | 14.42307692,0.000000,0.000000,0.000000,0.995897,0.000000,0.000000,0.090499 153 | 14.51923077,0.000000,0.000000,0.000000,0.997150,0.000000,0.000000,0.075447 154 | 14.61538462,0.000000,0.000000,0.000000,0.998176,0.000000,0.000000,0.060378 155 | 14.71153846,0.000000,0.000000,0.000000,0.998974,0.000000,0.000000,0.045296 156 | 14.80769231,0.000000,0.000000,0.000000,0.999544,0.000000,0.000000,0.030203 157 | 14.90384615,0.000000,0.000000,0.000000,0.999886,0.000000,0.000000,0.015103 158 | 15.0,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000 159 | 15.09615385,0.000000,0.000000,0.000000,0.999886,0.000000,0.000000,-0.015103 160 | 15.19230769,0.000000,0.000000,0.000000,0.999544,0.000000,0.000000,-0.030203 161 | 15.28846154,0.000000,0.000000,0.000000,0.998974,0.000000,0.000000,-0.045296 162 | 15.38461538,0.000000,0.000000,0.000000,0.998176,0.000000,0.000000,-0.060378 163 | 15.48076923,0.000000,0.000000,0.000000,0.997150,0.000000,0.000000,-0.075447 164 | 15.57692308,0.000000,0.000000,0.000000,0.995897,0.000000,0.000000,-0.090499 165 | 15.67307692,0.000000,0.000000,0.000000,0.994416,0.000000,0.000000,-0.105530 166 | 15.76923077,0.000000,0.000000,0.000000,0.992709,0.000000,0.000000,-0.120537 167 | 15.86538462,0.000000,0.000000,0.000000,0.990775,0.000000,0.000000,-0.135516 168 | 15.96153846,0.000000,0.000000,0.000000,0.988615,0.000000,0.000000,-0.150465 169 | 16.05769231,0.000000,0.000000,0.000000,0.986230,0.000000,0.000000,-0.165379 170 | 16.15384615,0.000000,0.000000,0.000000,0.983620,0.000000,0.000000,-0.180255 171 | 16.25,0.000000,0.000000,0.000000,0.980785,0.000000,0.000000,-0.195090 172 | 16.34615385,0.000000,0.000000,0.000000,0.977727,0.000000,0.000000,-0.209881 173 | 16.44230769,0.000000,0.000000,0.000000,0.974446,0.000000,0.000000,-0.224624 174 | 16.53846154,0.000000,0.000000,0.000000,0.970942,0.000000,0.000000,-0.239316 175 | 16.63461538,0.000000,0.000000,0.000000,0.967217,0.000000,0.000000,-0.253953 176 | 16.73076923,0.000000,0.000000,0.000000,0.963271,0.000000,0.000000,-0.268532 177 | 16.82692308,0.000000,0.000000,0.000000,0.959105,0.000000,0.000000,-0.283050 178 | 16.92307692,0.000000,0.000000,0.000000,0.954721,0.000000,0.000000,-0.297503 179 | 17.01923077,0.000000,0.000000,0.000000,0.950119,0.000000,0.000000,-0.311888 180 | 17.11538462,0.000000,0.000000,0.000000,0.945300,0.000000,0.000000,-0.326203 181 | 17.21153846,0.000000,0.000000,0.000000,0.940265,0.000000,0.000000,-0.340443 182 | 17.30769231,0.000000,0.000000,0.000000,0.935016,0.000000,0.000000,-0.354605 183 | 17.40384615,0.000000,0.000000,0.000000,0.929554,0.000000,0.000000,-0.368686 184 | 17.5,0.000000,0.000000,0.000000,0.923880,0.000000,0.000000,-0.382683 185 | 17.59615385,0.000000,0.000000,0.000000,0.929554,0.000000,0.000000,-0.368686 186 | 17.69230769,0.000000,0.000000,0.000000,0.935016,0.000000,0.000000,-0.354605 187 | 17.78846154,0.000000,0.000000,0.000000,0.940265,0.000000,0.000000,-0.340443 188 | 17.88461538,0.000000,0.000000,0.000000,0.945300,0.000000,0.000000,-0.326203 189 | 17.98076923,0.000000,0.000000,0.000000,0.950119,0.000000,0.000000,-0.311888 190 | 18.07692308,0.000000,0.000000,0.000000,0.954721,0.000000,0.000000,-0.297503 191 | 18.17307692,0.000000,0.000000,0.000000,0.959105,0.000000,0.000000,-0.283050 192 | 18.26923077,0.000000,0.000000,0.000000,0.963271,0.000000,0.000000,-0.268532 193 | 18.36538462,0.000000,0.000000,0.000000,0.967217,0.000000,0.000000,-0.253953 194 | 18.46153846,0.000000,0.000000,0.000000,0.970942,0.000000,0.000000,-0.239316 195 | 18.55769231,0.000000,0.000000,0.000000,0.974446,0.000000,0.000000,-0.224624 196 | 18.65384615,0.000000,0.000000,0.000000,0.977727,0.000000,0.000000,-0.209881 197 | 18.75,0.000000,0.000000,0.000000,0.980785,0.000000,0.000000,-0.195090 198 | 18.84615385,0.000000,0.000000,0.000000,0.983620,0.000000,0.000000,-0.180255 199 | 18.94230769,0.000000,0.000000,0.000000,0.986230,0.000000,0.000000,-0.165379 200 | 19.03846154,0.000000,0.000000,0.000000,0.988615,0.000000,0.000000,-0.150465 201 | 19.13461538,0.000000,0.000000,0.000000,0.990775,0.000000,0.000000,-0.135516 202 | 19.23076923,0.000000,0.000000,0.000000,0.992709,0.000000,0.000000,-0.120537 203 | 19.32692308,0.000000,0.000000,0.000000,0.994416,0.000000,0.000000,-0.105530 204 | 19.42307692,0.000000,0.000000,0.000000,0.995897,0.000000,0.000000,-0.090499 205 | 19.51923077,0.000000,0.000000,0.000000,0.997150,0.000000,0.000000,-0.075447 206 | 19.61538462,0.000000,0.000000,0.000000,0.998176,0.000000,0.000000,-0.060378 207 | 19.71153846,0.000000,0.000000,0.000000,0.998974,0.000000,0.000000,-0.045296 208 | 19.80769231,0.000000,0.000000,0.000000,0.999544,0.000000,0.000000,-0.030203 209 | 19.90384615,0.000000,0.000000,0.000000,0.999886,0.000000,0.000000,-0.015103 210 | 20.0,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000 211 | -------------------------------------------------------------------------------- /cppflow/paths/s_truncated.csv: -------------------------------------------------------------------------------- 1 | time,x,y,z,qw,qx,qy,qz 2 | 0.0,0.000000,0.000000,-0.285000,1.000000,0.000000,0.000000,0.000000 3 | 0.06666667,0.000000,0.000000,-0.290000,1.000000,0.000000,0.000000,0.000000 4 | 0.13333333,0.000000,0.000000,-0.295000,1.000000,0.000000,0.000000,0.000000 5 | 0.2,0.000000,0.000000,-0.300000,1.000000,0.000000,0.000000,0.000000 6 | 0.26666667,0.000000,-0.005000,-0.300000,1.000000,0.000000,0.000000,0.000000 7 | 0.33333333,0.000000,-0.010000,-0.300000,1.000000,0.000000,0.000000,0.000000 8 | 0.4,0.000000,-0.015000,-0.300000,1.000000,0.000000,0.000000,0.000000 9 | 0.46666667,0.000000,-0.020000,-0.300000,1.000000,0.000000,0.000000,0.000000 10 | 0.53333333,0.000000,-0.025000,-0.300000,1.000000,0.000000,0.000000,0.000000 11 | 0.6,0.000000,-0.030000,-0.300000,1.000000,0.000000,0.000000,0.000000 12 | 0.66666667,0.000000,-0.035000,-0.300000,1.000000,0.000000,0.000000,0.000000 13 | 0.73333333,0.000000,-0.040000,-0.300000,1.000000,0.000000,0.000000,0.000000 14 | 0.8,0.000000,-0.045000,-0.300000,1.000000,0.000000,0.000000,0.000000 15 | 0.86666667,0.000000,-0.050000,-0.300000,1.000000,0.000000,0.000000,0.000000 16 | 0.93333333,0.000000,-0.055000,-0.300000,1.000000,0.000000,0.000000,0.000000 17 | 1.0,0.000000,-0.060000,-0.300000,1.000000,0.000000,0.000000,0.000000 18 | 1.06666667,0.000000,-0.065000,-0.300000,1.000000,0.000000,0.000000,0.000000 19 | 1.13333333,0.000000,-0.070000,-0.300000,1.000000,0.000000,0.000000,0.000000 20 | 1.2,0.000000,-0.075000,-0.300000,1.000000,0.000000,0.000000,0.000000 21 | 1.26666667,0.000000,-0.080000,-0.300000,1.000000,0.000000,0.000000,0.000000 22 | 1.33333333,0.000000,-0.085000,-0.300000,1.000000,0.000000,0.000000,0.000000 23 | 1.4,0.000000,-0.090000,-0.300000,1.000000,0.000000,0.000000,0.000000 24 | 1.46666667,0.000000,-0.095000,-0.300000,1.000000,0.000000,0.000000,0.000000 25 | 1.53333333,0.000000,-0.100000,-0.300000,1.000000,0.000000,0.000000,0.000000 26 | 1.6,0.000000,-0.105000,-0.300000,1.000000,0.000000,0.000000,0.000000 27 | 1.66666667,0.000000,-0.110000,-0.300000,1.000000,0.000000,0.000000,0.000000 28 | 1.73333333,0.000000,-0.115000,-0.300000,1.000000,0.000000,0.000000,0.000000 29 | 1.8,0.000000,-0.120000,-0.300000,1.000000,0.000000,0.000000,0.000000 30 | 1.86666667,0.000000,-0.125000,-0.300000,1.000000,0.000000,0.000000,0.000000 31 | 1.93333333,0.000000,-0.130000,-0.300000,1.000000,0.000000,0.000000,0.000000 32 | 2.0,0.000000,-0.135000,-0.300000,1.000000,0.000000,0.000000,0.000000 33 | 2.06666667,0.000000,-0.140000,-0.300000,1.000000,0.000000,0.000000,0.000000 34 | 2.13333333,0.000000,-0.145000,-0.300000,1.000000,0.000000,0.000000,0.000000 35 | 2.2,0.000000,-0.150000,-0.300000,1.000000,0.000000,0.000000,0.000000 36 | 2.26666667,0.000000,-0.155000,-0.300000,1.000000,0.000000,0.000000,0.000000 37 | 2.33333333,0.000000,-0.160000,-0.300000,1.000000,0.000000,0.000000,0.000000 38 | 2.4,0.000000,-0.165000,-0.300000,1.000000,0.000000,0.000000,0.000000 39 | 2.46666667,0.000000,-0.170000,-0.300000,1.000000,0.000000,0.000000,0.000000 40 | 2.53333333,0.000000,-0.175000,-0.300000,1.000000,0.000000,0.000000,0.000000 41 | 2.6,0.000000,-0.180000,-0.300000,1.000000,0.000000,0.000000,0.000000 42 | 2.66666667,0.000000,-0.185000,-0.300000,1.000000,0.000000,0.000000,0.000000 43 | 2.73333333,0.000000,-0.190000,-0.300000,1.000000,0.000000,0.000000,0.000000 44 | 2.8,0.000000,-0.195000,-0.300000,1.000000,0.000000,0.000000,0.000000 45 | 2.86666667,0.000000,-0.200000,-0.300000,1.000000,0.000000,0.000000,0.000000 46 | 2.93333333,0.000000,-0.205000,-0.300000,1.000000,0.000000,0.000000,0.000000 47 | 3.0,0.000000,-0.210000,-0.300000,1.000000,0.000000,0.000000,0.000000 48 | 3.06666667,0.000000,-0.215000,-0.300000,1.000000,0.000000,0.000000,0.000000 49 | 3.13333333,0.000000,-0.220000,-0.300000,1.000000,0.000000,0.000000,0.000000 50 | 3.2,0.000000,-0.225000,-0.300000,1.000000,0.000000,0.000000,0.000000 51 | 3.26666667,0.000000,-0.230000,-0.300000,1.000000,0.000000,0.000000,0.000000 52 | 3.33333333,0.000000,-0.235000,-0.300000,1.000000,0.000000,0.000000,0.000000 53 | 3.4,0.000000,-0.240000,-0.300000,1.000000,0.000000,0.000000,0.000000 54 | 3.46666667,0.000000,-0.245000,-0.300000,1.000000,0.000000,0.000000,0.000000 55 | 3.53333333,0.000000,-0.250000,-0.300000,1.000000,0.000000,0.000000,0.000000 56 | 3.6,0.000000,-0.255000,-0.300000,1.000000,0.000000,0.000000,0.000000 57 | 3.66666667,0.000000,-0.260000,-0.300000,1.000000,0.000000,0.000000,0.000000 58 | 3.73333333,0.000000,-0.265000,-0.300000,1.000000,0.000000,0.000000,0.000000 59 | 3.8,0.000000,-0.270000,-0.300000,1.000000,0.000000,0.000000,0.000000 60 | 3.86666667,0.000000,-0.275000,-0.300000,1.000000,0.000000,0.000000,0.000000 -------------------------------------------------------------------------------- /cppflow/paths/update_path_format.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | PATH_TIME = 20 4 | 5 | def update_path(filename): 6 | 7 | filepath = f"paths_torm/{filename}" 8 | with open(filepath, "r") as f: 9 | lines = [line.strip("\n") for line in f.readlines()] 10 | 11 | times = np.linspace(0, PATH_TIME, len(lines)) 12 | 13 | with open(filename+".csv", "w", newline="\n") as f: 14 | f.write("time,x,y,z,qw,qx,qy,qz"+"\n") 15 | 16 | for idx, line in enumerate(lines): 17 | _, xyz, q = line.split(";") 18 | f.write(str(round(float(times[idx]), 8)) + "," + xyz+","+q+"\n") 19 | 20 | 21 | for filename in ["circle", "hello", "rot_yz", "s", "square"]: 22 | update_path(filename) -------------------------------------------------------------------------------- /cppflow/problems/README.md: -------------------------------------------------------------------------------- 1 | 2 | # Problems 3 | Problems are directly copied from 'TORM's github repo (https://github.com/cheulkang/TORM/tree/main/src/data/problem) so that CppFlow will have a fair comparison against the other authors work. 4 | 5 | problem name | includes obstacles? 6 | -------------|-------------------- 7 | fetch_circle | yes 8 | fetch_hello | no 9 | fetch_rotation | no 10 | fetch_s_two | yes 11 | fetch_s | yes 12 | fetch_square | yes 13 | 14 | 15 | 16 | ## Notes 17 | - The difference between the 's' and 's_two' paths is unclear. Also unclear is which one was used in the paper. 18 | 19 | ## Path offsets 20 | For all problems with the fetch robot, the `path__offset` value is copied from the TORM repository. Note that the offset is named 'start_pose' there (see https://github.com/cheulkang/TORM/blob/main/src/data/problem/fetch_square.yaml#L8). 21 | 22 | -------------------------------------------------------------------------------- /cppflow/problems/fetch__circle.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "circle" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.9, 0.25, 0.46] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | 12 | obstacle_xyz_offset: [0.0, 0.0, 0.0] 13 | obstacles: [[x: 0.4, y: 0.4, z: 0.825, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.05, size_z: 0.8], 14 | [x: 0.4, y: -0.4, z: 0.825, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.05, size_z: 0.8], 15 | [x: 0.4, y: 0.0, z: 1.225, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.85, size_z: 0.05], 16 | [x: 0.4, y: 0.0, z: 0.425, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.85, size_z: 0.05]] 17 | -------------------------------------------------------------------------------- /cppflow/problems/fetch__hello.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "hello" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.45, 0.25] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] -------------------------------------------------------------------------------- /cppflow/problems/fetch__rot_yz.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "rot_yz" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.0, 0.35] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | -------------------------------------------------------------------------------- /cppflow/problems/fetch__rot_yz2.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "rot_yz2" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.0, 0.35] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | -------------------------------------------------------------------------------- /cppflow/problems/fetch__s.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "s" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [1.0, 0.3, 0.55] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | 12 | obstacle_xyz_offset: [0.0, 0.0, -0.2] 13 | obstacles: [[x: 0.75, y: 0.0, z: 0.36, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.70, size_y: 1.40, size_z: 0.72], 14 | [x: 0.55, y: -0.155, z: 0.8275, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.055, size_z: 0.215]] -------------------------------------------------------------------------------- /cppflow/problems/fetch__square.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "square" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [1.1, 0.0, 0.66] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | 12 | obstacle_xyz_offset: [0.0, 0.0, 0.0] 13 | # name, x, y, z, roll, pitch, yaw, size_x, size_y, size_z 14 | obstacles: [[x: 0.75, y: 0.0, z: 0.36, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.70, size_y: 1.40, size_z: 0.72]] -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__circle.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "circle" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.9, 0.25, 0.46] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | 12 | obstacle_xyz_offset: [0.0, 0.0, 0.0] 13 | obstacles: [[x: 0.4, y: 0.4, z: 0.825, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.05, size_z: 0.8], 14 | [x: 0.4, y: -0.4, z: 0.825, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.05, size_z: 0.8], 15 | [x: 0.4, y: 0.0, z: 1.225, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.85, size_z: 0.05], 16 | [x: 0.4, y: 0.0, z: 0.425, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.85, size_z: 0.05]] 17 | -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__hello.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "hello" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.45, 0.25] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__hello_mini.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "hello_mini" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.45, 0.25] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__rot_yz.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "rot_yz" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.0, 0.35] 6 | path_R_offset: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 7 | -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__rot_yz2.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "rot_yz2" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [0.8, 0.0, 0.35] 6 | path_R_offset: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 7 | -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__s.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "s" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [1.0, 0.3, 0.55] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | 12 | obstacle_xyz_offset: [0.0, 0.0, -0.2] 13 | obstacles: [[x: 0.75, y: 0.0, z: 0.36, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.70, size_y: 1.40, size_z: 0.72], 14 | [x: 0.55, y: -0.155, z: 0.8275, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.3, size_y: 0.055, size_z: 0.215]] -------------------------------------------------------------------------------- /cppflow/problems/fetch_arm__square.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "square" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [1.1, 0.0, 0.66] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] 11 | 12 | obstacle_xyz_offset: [0.0, 0.0, 0.0] 13 | # name, x, y, z, roll, pitch, yaw, size_x, size_y, size_z 14 | obstacles: [[x: 0.75, y: 0.0, z: 0.36, roll: 0.0, pitch: 0.0, yaw: 0.0, size_x: 0.70, size_y: 1.40, size_z: 0.72]] -------------------------------------------------------------------------------- /cppflow/problems/panda__1cube.yaml: -------------------------------------------------------------------------------- 1 | robot: "panda" 2 | path_name: "1cube" 3 | 4 | path_offset_frame: "world" 5 | path_xyz_offset: [0, 0.5421984559194368, 0.7885155964931997] 6 | path_R_offset: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 7 | 8 | obstacle_xyz_offset: [0, 0, 0] 9 | obstacles: 10 | [ 11 | [ 12 | size_x: 0.25, 13 | size_y: 0.25, 14 | size_z: 0.25, 15 | x: 0.0, 16 | y: 0.2, 17 | z: 0.7, 18 | roll: 0.0, 19 | pitch: 0.0, 20 | yaw: 0.0, 21 | ], 22 | ] 23 | -------------------------------------------------------------------------------- /cppflow/problems/panda__1cube_mini.yaml: -------------------------------------------------------------------------------- 1 | robot: "panda" 2 | path_name: "1cube_mini" 3 | 4 | path_offset_frame: "world" 5 | path_xyz_offset: [0, 0.5421984559194368, 0.7885155964931997] 6 | path_R_offset: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 7 | 8 | obstacle_xyz_offset: [0, 0, 0] 9 | obstacles: 10 | [ 11 | [ 12 | size_x: 0.25, 13 | size_y: 0.25, 14 | size_z: 0.25, 15 | x: 0.0, 16 | y: 0.2, 17 | z: 0.7, 18 | roll: 0.0, 19 | pitch: 0.0, 20 | yaw: 0.0, 21 | ], 22 | ] 23 | -------------------------------------------------------------------------------- /cppflow/problems/panda__2cubes.yaml: -------------------------------------------------------------------------------- 1 | robot: "panda" 2 | path_name: "2cubes" 3 | 4 | path_offset_frame: "world" 5 | path_xyz_offset: [0.0, 0.54, 0.79] 6 | path_R_offset: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 7 | 8 | obstacle_xyz_offset: [0, 0, 0] 9 | obstacles: 10 | [ 11 | [ 12 | x: 0.2, y: 0.3, z: 0.4, 13 | roll: 0.0, 14 | pitch: 0.0, 15 | yaw: 0.0, 16 | size_x: 0.15, 17 | size_y: 0.15, 18 | size_z: 0.15, 19 | ], 20 | [ 21 | x: -0.25, y: 0.3, z: 0.75, 22 | roll: 0.0, 23 | pitch: 0.0, 24 | yaw: 0.0, 25 | size_x: 0.15, 26 | size_y: 0.15, 27 | size_z: 0.15, 28 | ], 29 | ] 30 | -------------------------------------------------------------------------------- /cppflow/problems/panda__flappy_bird.yaml: -------------------------------------------------------------------------------- 1 | robot: "panda" 2 | path_name: "flappy_bird" 3 | 4 | path_offset_frame: "world" 5 | path_xyz_offset: [0.0, 0.54, 0.79] 6 | path_R_offset: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 7 | 8 | 9 | obstacle_xyz_offset: [0, 0, 0] 10 | obstacles: 11 | [ 12 | [ 13 | roll: 0.0, 14 | pitch: 0.0, 15 | yaw: 0.0, 16 | size_x: 0.05, size_y: 0.15, size_z: 0.3, 17 | # x: 0.0, y: 0.25, z: 1.05, # change to 1.05 to make easier 18 | x: 0.0, y: 0.25, z: 1.0, 19 | ], 20 | [ 21 | roll: 0.0, 22 | pitch: 0.0, 23 | yaw: 0.0, 24 | size_x: 0.05, size_y: 0.15, size_z: 0.3, 25 | x: 0.0, y: 0.25, z: 0.5, 26 | ], 27 | ] 28 | -------------------------------------------------------------------------------- /cppflow/problems/panda__square.yaml: -------------------------------------------------------------------------------- 1 | 2 | robot: "panda" 3 | path_name: "square" 4 | 5 | path_offset_frame: "world" 6 | path_xyz_offset: [0.75, 0, 0.5] 7 | path_R_offset: [ 8 | [0, 0, 1], 9 | [0, 1, 0], 10 | [-1, 0, 0] 11 | ] # rotation about +y by 90deg. This makes the gripper pointing in the same direction as the fetch robot is in the TORM demo 12 | -------------------------------------------------------------------------------- /cppflow/ros2/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jstmn/cppflow/c92e0a45aeee257a9a489b49cf83370193fe5844/cppflow/ros2/__init__.py -------------------------------------------------------------------------------- /cppflow/ros2/cppflow: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jstmn/cppflow/c92e0a45aeee257a9a489b49cf83370193fe5844/cppflow/ros2/cppflow -------------------------------------------------------------------------------- /cppflow/ros2/resources/CppFlowEnvironmentConfig_request.bin: -------------------------------------------------------------------------------- 1 |  panda_link0 panda_handpanda -------------------------------------------------------------------------------- /cppflow/ros2/resources/CppFlowQuery_request.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jstmn/cppflow/c92e0a45aeee257a9a489b49cf83370193fe5844/cppflow/ros2/resources/CppFlowQuery_request.bin -------------------------------------------------------------------------------- /cppflow/ros2/resources/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jstmn/cppflow/c92e0a45aeee257a9a489b49cf83370193fe5844/cppflow/ros2/resources/__init__.py -------------------------------------------------------------------------------- /cppflow/ros2/ros2_publisher.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | import numpy as np 3 | import importlib.resources as resources 4 | import warnings 5 | 6 | import rclpy 7 | from rclpy.node import Node 8 | from rclpy.serialization import deserialize_message 9 | from cppflow_msgs.srv import CppFlowQuery, CppFlowEnvironmentConfig 10 | from cppflow_msgs.msg import CppFlowProblem 11 | from geometry_msgs.msg import Pose, Point, Quaternion 12 | from sensor_msgs.msg import JointState 13 | from jrl.robot import Robot 14 | from jrl.robots import Panda 15 | 16 | 17 | def _get_initial_configuration(robot: Robot, target_pose: Pose): 18 | warnings.warn("No collision checking is performed against obstacles in the scene to find an initial configuration.") 19 | for _ in range(25): 20 | target_pose_np = np.array( 21 | [ 22 | target_pose.position.x, 23 | target_pose.position.y, 24 | target_pose.position.z, 25 | target_pose.orientation.w, 26 | target_pose.orientation.x, 27 | target_pose.orientation.y, 28 | target_pose.orientation.z, 29 | ] 30 | ) 31 | initial_configuration = robot.inverse_kinematics_klampt(target_pose_np, positional_tolerance=5e-5)[0] 32 | if not robot.config_self_collides(initial_configuration): 33 | return initial_configuration 34 | raise RuntimeError("Could not find collision free initial configuration") 35 | 36 | 37 | class CppFlowQueryClient(Node): 38 | def __init__(self): 39 | super().__init__("cppflow_publisher") 40 | 41 | self.planning_client = self.create_client(CppFlowQuery, "/cppflow_planning_query") 42 | while not self.planning_client.wait_for_service(timeout_sec=1.0): 43 | self.get_logger().info("Waiting for service /cppflow_planning_query to be available...") 44 | 45 | self.scene_configuration_client = self.create_client( 46 | CppFlowEnvironmentConfig, "/cppflow_environment_configuration" 47 | ) 48 | while not self.scene_configuration_client.wait_for_service(timeout_sec=1.0): 49 | self.get_logger().info("Waiting for service /cppflow_environment_configuration to be available...") 50 | 51 | self.panda: Optional[Panda] = None 52 | self.send_scene_configuration_request() 53 | self.send_dummy_problem_planning_request() 54 | # self.send_cached_problem_planning_request() 55 | 56 | def send_scene_configuration_request(self): 57 | 58 | # Example setup for the request fields 59 | request = CppFlowEnvironmentConfig.Request() 60 | request.base_frame = "panda_link0" 61 | request.end_effector_frame = "panda_hand" 62 | request.jrl_robot_name = "panda" 63 | 64 | # Send request 65 | future = self.scene_configuration_client.call_async(request) 66 | rclpy.spin_until_future_complete(self, future) 67 | try: 68 | response = future.result() 69 | self.get_logger().info(f"Received response: {response}") 70 | except Exception as e: 71 | self.get_logger().error(f"Service call failed: {str(e)}") 72 | 73 | def send_cached_problem_planning_request(self): 74 | # Load CppFlowQuery_request.bin 75 | with resources.open_binary("cppflow.ros2.resources", "CppFlowQuery_request.bin") as f: 76 | request = deserialize_message(f.read(), CppFlowQuery.Request) 77 | request.verbosity = 2 78 | self.get_logger().info("Loaded cached problem request 'CppFlowQuery_request.bin'") 79 | self._send_planning_request(request) 80 | 81 | def send_dummy_problem_planning_request(self): 82 | 83 | if self.panda is None: 84 | self.panda = Panda() 85 | 86 | request = CppFlowQuery.Request() 87 | 88 | # Example setup for the request fields 89 | request.base_frame = "panda_link0" 90 | request.end_effector_frame = "panda_hand" 91 | request.jrl_robot_name = "panda" 92 | request.verbosity = 1 93 | request.max_planning_time_sec = 3.0 94 | request.anytime_mode_enabled = False 95 | request.max_allowed_position_error_cm = 0.1 96 | request.max_allowed_rotation_error_deg = 1.0 97 | request.max_allowed_mjac_deg = 2.5 98 | request.max_allowed_mjac_cm = 0.5 99 | 100 | # Create an example CppFlowProblem with waypoints 101 | # This is the beginning of the panda__1cube problem 102 | xyz_offset = np.array([0, 0.5421984559194368, 0.7885155964931997]) 103 | # x, y, z, qw, x, y, z 104 | target_path = np.array( 105 | [ 106 | [0.45, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 107 | [0.44547737, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 108 | [0.44095477, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 109 | [0.43643215, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 110 | [0.43190953, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 111 | [0.4273869, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 112 | [0.42286432, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 113 | [0.4183417, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 114 | [0.41381907, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 115 | [0.40929648, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 116 | [0.40477386, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 117 | ] 118 | ) 119 | 120 | problem = CppFlowProblem() 121 | problem.waypoints = [] 122 | for waypoint in target_path: 123 | xyz = xyz_offset + waypoint[0:3] 124 | qxyz = waypoint[3:] 125 | waypoint = Pose( 126 | position=Point(x=xyz[0], y=xyz[1], z=xyz[2]), 127 | orientation=Quaternion(x=qxyz[1], y=qxyz[2], z=qxyz[3], w=qxyz[0]), 128 | ) 129 | problem.waypoints.append(waypoint) 130 | 131 | request.problems = [problem] 132 | request.initial_configuration_is_set = True 133 | request.initial_configuration = JointState( 134 | position=_get_initial_configuration(self.panda, request.problems[0].waypoints[0]) 135 | ) 136 | print("request.initial_configuration:", request.initial_configuration.position) 137 | 138 | # Send request 139 | self._send_planning_request(request) 140 | 141 | def _send_planning_request(self, request): 142 | future = self.planning_client.call_async(request) 143 | rclpy.spin_until_future_complete(self, future) 144 | try: 145 | response = future.result() 146 | self.get_logger().info("Received CppFlowQuery.Response") 147 | for i, (trajectory, success, error) in enumerate( 148 | zip(response.trajectories, response.success, response.errors) 149 | ): 150 | self.get_logger().info(f"Problem {i}: Success = {success}, Error = {error}") 151 | self.get_logger().info(f"Trajectory {i}: {trajectory.joint_names}, {len(trajectory.points)} points") 152 | for j, point in enumerate(trajectory.points): 153 | self.get_logger().info(f" {j}: {point.positions}") 154 | except Exception as e: 155 | self.get_logger().error(f"Service call failed: {str(e)}") 156 | 157 | 158 | """ Usage 159 | ros2 run cppflow ros2_subscriber # terminal 1 160 | ros2 run cppflow ros2_publisher # terminal 2 161 | 162 | # 163 | cp /tmp/CppFlow*.bin cppflow/ros2/resources/ 164 | """ 165 | 166 | 167 | def main(args=None): 168 | rclpy.init(args=args) 169 | client = CppFlowQueryClient() 170 | client.destroy_node() 171 | rclpy.shutdown() 172 | 173 | 174 | if __name__ == "__main__": 175 | main() 176 | -------------------------------------------------------------------------------- /cppflow/ros2/ros2_subscriber.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | from time import time 3 | import traceback 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | from rclpy.serialization import serialize_message 8 | from cppflow_msgs.msg import CppFlowProblem 9 | from cppflow_msgs.srv import CppFlowQuery, CppFlowEnvironmentConfig 10 | from jrl.robot import Robot 11 | from jrl.robots import get_robot 12 | from jrl.utils import to_torch 13 | 14 | from cppflow.data_types import Problem 15 | from cppflow.ros2.ros2_utils import waypoints_to_se3_sequence, plan_to_ros_trajectory 16 | from cppflow.planners import PlannerSearcher, CppFlowPlanner, Planner 17 | from cppflow.data_types import PlannerSettings, Constraints 18 | from cppflow.utils import set_seed 19 | from cppflow.collision_detection import qpaths_batched_self_collisions, qpaths_batched_env_collisions 20 | 21 | set_seed() 22 | 23 | SAVE_MESSAGES = True 24 | 25 | 26 | PLANNERS = { 27 | "CppFlowPlanner": CppFlowPlanner, 28 | "PlannerSearcher": PlannerSearcher, 29 | } 30 | 31 | 32 | PLANNER_SETTINGS = { 33 | "CppFlowPlanner": PlannerSettings( 34 | k=175, 35 | tmax_sec=5.0, 36 | anytime_mode_enabled=False, 37 | do_rerun_if_large_dp_search_mjac=True, 38 | do_rerun_if_optimization_fails=True, 39 | do_return_search_path_mjac=True, 40 | ), 41 | "PlannerSearcher": {"k": 175, "verbosity": 0}, 42 | } 43 | PLANNER = "CppFlowPlanner" 44 | 45 | 46 | class SubscriberNode(Node): 47 | def __init__(self): 48 | super().__init__("cppflow_query_server") 49 | self.srv = self.create_service(CppFlowQuery, "/cppflow_planning_query", self.planning_query_callback) 50 | self.environment_setup_srv = self.create_service( 51 | CppFlowEnvironmentConfig, "/cppflow_environment_configuration", self.environment_setup_callback 52 | ) 53 | 54 | self.get_logger().info("CppFlowQuery service server started...") 55 | self.robot: Optional[Robot] = None 56 | self.planner: Optional[Planner] = None 57 | self.obstacles = [] 58 | 59 | def environment_setup_callback(self, request, response): 60 | t0 = time() 61 | self.get_logger().info(f"Received a CppFlowEnvironmentConfig message: {request}") 62 | 63 | if SAVE_MESSAGES: 64 | save_filepath = "/tmp/CppFlowEnvironmentConfig_request.bin" 65 | with open(save_filepath, "wb") as file: 66 | file.write(serialize_message(request)) 67 | self.get_logger().info(f"Saved CppFlowEnvironmentConfig request to '{save_filepath}'") 68 | 69 | def specify_malformed_query(msg: str): 70 | response.success = False 71 | response.error = msg 72 | self.get_logger().info(f"Returning response to malformed query: {response}") 73 | return response 74 | 75 | # Get robot 76 | if (self.robot is None) or (self.robot.name != request.jrl_robot_name): 77 | try: 78 | t0_robot = time() 79 | self.robot = get_robot(request.jrl_robot_name) 80 | self.get_logger().info(f"Loaded robot '{self.robot.name}' in {time() - t0_robot:.3f} seconds") 81 | except ValueError: 82 | return specify_malformed_query(f"Robot '{request.jrl_robot_name}' doesn't exist in the Jrl library") 83 | 84 | # end effector frame doesn't match 85 | if self.robot.end_effector_link_name != request.end_effector_frame: 86 | error = ( 87 | f"The provided dnd-effector frame '{request.end_effector_frame}' does not match the robot's" 88 | f" end-effector link '{self.robot.end_effector_link_name}" 89 | ) 90 | return specify_malformed_query(error) 91 | 92 | # base link doesn't match 93 | robot_base_link_name = self.robot._end_effector_kinematic_chain[0].parent 94 | if robot_base_link_name != request.base_frame: 95 | error = ( 96 | f"The provided base frame '{request.base_frame}' does not match the robot's base link" 97 | f" '{robot_base_link_name}" 98 | ) 99 | return specify_malformed_query(error) 100 | 101 | self.obstacles = request.obstacles 102 | # TODO: redesign API, so that planner settings are configured here instead of being hardcoded. Note that the 103 | # correct settings are configured with set_settings() below 104 | self.planner = PLANNERS[PLANNER](PLANNER_SETTINGS["CppFlowPlanner"], self.robot) 105 | response.success = True 106 | self.get_logger().info(f"Returning response: {response} ({time() - t0:.3f} seconds)") 107 | return response 108 | 109 | def planning_query_callback(self, request, response): 110 | t0 = time() 111 | 112 | def specify_malformed_query(msg: str): 113 | response.is_malformed_query = True 114 | response.malformed_query_error = msg 115 | self.get_logger().info(f"Returning response: {response} for malformed query") 116 | return response 117 | 118 | if SAVE_MESSAGES: 119 | save_filepath = "/tmp/CppFlowQuery_request.bin" 120 | with open(save_filepath, "wb") as file: 121 | file.write(serialize_message(request)) 122 | self.get_logger().info(f"Saved a CppFlowQuery request to '{save_filepath}'") 123 | 124 | if len(request.problems) != 1: 125 | return specify_malformed_query( 126 | f"Only 1 planning problem per query currently supported ({len(request.problems)} problems provided)" 127 | ) 128 | 129 | if request.max_planning_time_sec < 1e-6: 130 | return specify_malformed_query( 131 | f"Planning time is too short (`max_planning_time_sec`: {request.max_planning_time_sec})" 132 | ) 133 | 134 | if self.planner is None: 135 | return specify_malformed_query( 136 | "Planner has not been configured. Send a 'CppFlowEnvironmentConfig' message on the" 137 | " '/cppflow_environment_configuration' topic to configure the scene first." 138 | ) 139 | 140 | request_problem: CppFlowProblem = request.problems[0] 141 | 142 | if len(request_problem.waypoints) < 3: 143 | return specify_malformed_query( 144 | f"At least 3 waypoints are required per problem (only {len(request_problem.waypoints)} provided)" 145 | ) 146 | 147 | ndof = self.planner.robot.ndof 148 | settings = PLANNER_SETTINGS[PLANNER] 149 | settings.tmax_sec = 0.9 * request.max_planning_time_sec 150 | settings.verbosity = request.verbosity 151 | settings.anytime_mode_enabled = request.anytime_mode_enabled 152 | constraints = Constraints( 153 | max_allowed_position_error_cm=request.max_allowed_position_error_cm, 154 | max_allowed_rotation_error_deg=request.max_allowed_rotation_error_deg, 155 | max_allowed_mjac_deg=request.max_allowed_mjac_deg, 156 | max_allowed_mjac_cm=request.max_allowed_mjac_cm, 157 | ) 158 | self.planner.set_settings(settings) 159 | 160 | # TODO: Add obstacles 161 | q0 = ( 162 | to_torch(request.initial_configuration.position).view(1, ndof) 163 | if request.initial_configuration_is_set 164 | else None 165 | ) 166 | try: 167 | problem = Problem( 168 | constraints, 169 | target_path=waypoints_to_se3_sequence(request_problem.waypoints), 170 | initial_configuration=q0, 171 | robot=self.robot, 172 | name="ros2-queried-problem", 173 | full_name="ros2-queried-problem", 174 | obstacles=[], 175 | obstacles_Tcuboids=[], 176 | obstacles_cuboids=[], 177 | obstacles_klampt=[], 178 | ) 179 | self.get_logger().info( 180 | f"target-path cumulative positional-change, cm: {problem.path_length_cumultive_positional_change_cm}" 181 | ) 182 | self.get_logger().info( 183 | f"target-path cumulative rotational-change, deg: {problem.path_length_cumulative_rotational_change_deg}" 184 | ) 185 | self.get_logger().info( 186 | f"target-path mean positional change per waypoint, cm: {problem.path_length_cumultive_positional_change_cm / problem.n_timesteps}" 187 | ) 188 | self.get_logger().info( 189 | f"target-path mean rotational change per waypoint, deg: {problem.path_length_cumulative_rotational_change_deg / problem.n_timesteps}" 190 | ) 191 | except AssertionError as e: 192 | return specify_malformed_query(f"Creating 'Problem' dataclass failed: {e}") 193 | 194 | # Check if initial configuration is valid 195 | if q0 is not None: 196 | if qpaths_batched_env_collisions(problem, q0.view(1, 1, ndof)).item(): 197 | return specify_malformed_query("Initial configuration is in collision with environment") 198 | if qpaths_batched_self_collisions(problem, q0.view(1, 1, ndof)).item(): 199 | return specify_malformed_query("Initial configuration is self-colliding") 200 | 201 | try: 202 | planning_result = self.planner.generate_plan(problem) 203 | except (RuntimeError, AttributeError) as e: 204 | tb = traceback.extract_tb(e.__traceback__)[-1] 205 | filename = tb.filename 206 | line_number = tb.lineno 207 | error_msg = f"{e} (File: {filename}, Line: {line_number})" 208 | response.trajectories = [] 209 | response.success = [False] 210 | response.errors = [error_msg] 211 | self.get_logger().info(f"Planning failed with exception: '{error_msg}'") 212 | return response 213 | 214 | # Write output to 'response' 215 | plan = planning_result.plan 216 | response.trajectories = [plan_to_ros_trajectory(plan, self.robot)] 217 | response.success = [plan.is_valid] 218 | response.errors = [""] 219 | self.get_logger().info( 220 | f"Planning complete - returning {sum(response.success)} / {len(response.trajectories)} successful" 221 | f" trajectories ({time() - t0:.3f} seconds)" 222 | ) 223 | self.get_logger().info(f"{planning_result.plan}") 224 | self.get_logger().info(f"{planning_result.timing}") 225 | return response 226 | 227 | 228 | """ Usage 229 | 230 | ros2 run cppflow ros2_subscriber 231 | """ 232 | 233 | 234 | def main(args=None): 235 | rclpy.init() 236 | node = SubscriberNode() 237 | try: 238 | rclpy.spin(node) 239 | except KeyboardInterrupt: 240 | pass 241 | finally: 242 | node.destroy_node() 243 | rclpy.shutdown() 244 | 245 | 246 | if __name__ == "__main__": 247 | main() 248 | -------------------------------------------------------------------------------- /cppflow/ros2/ros2_utils.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | 3 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 4 | from geometry_msgs.msg import Pose 5 | from jrl.robot import Robot 6 | import torch 7 | import rclpy 8 | 9 | from cppflow.data_types import Plan 10 | 11 | 12 | def waypoints_to_se3_sequence(waypoints: List[Pose]) -> torch.Tensor: 13 | """Convert a list of waypoints into a tensor representing the SE(3) path. 14 | 15 | Args: 16 | waypoints (List[Pose]): List of N waypoints in the form of Pose messages. 17 | 18 | Returns: 19 | torch.Tensor: Tensor representing the target path with dimensions (N, 7) and format x, y, z, qw, qx, qy, qz. 20 | """ 21 | target_path = torch.zeros((len(waypoints), 7)) 22 | 23 | for i, waypoint in enumerate(waypoints): 24 | target_path[i, :] = torch.tensor( 25 | [ 26 | waypoint.position.x, 27 | waypoint.position.y, 28 | waypoint.position.z, 29 | waypoint.orientation.w, 30 | waypoint.orientation.x, 31 | waypoint.orientation.y, 32 | waypoint.orientation.z, 33 | ] 34 | ) 35 | return target_path 36 | 37 | 38 | def plan_to_ros_trajectory(plan: Plan, robot: Robot) -> JointTrajectory: 39 | """Convert a CppFlow plan into a ROS JointTrajectory message.""" 40 | trajectory = JointTrajectory() 41 | trajectory.header.stamp = rclpy.time.Time().to_msg() # Converts current time to a `Time` message 42 | trajectory.joint_names = robot.actuated_joint_names 43 | 44 | zero_velocity = [0.0] * robot.ndof 45 | for i in range(plan.q_path.shape[0]): 46 | point = JointTrajectoryPoint() 47 | point.positions = plan.q_path[i].cpu().tolist() 48 | point.velocities = zero_velocity 49 | point.time_from_start.sec = i 50 | point.time_from_start.nanosec = 12 51 | trajectory.points.append(point) 52 | return trajectory 53 | -------------------------------------------------------------------------------- /cppflow/search.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | import warnings 3 | 4 | import numpy as np 5 | import torch 6 | import matplotlib.pyplot as plt 7 | from jrl.robot import Robot 8 | 9 | from cppflow.data_types import Problem 10 | from cppflow.config import DEVICE 11 | from cppflow.utils import TimerContext, cm_to_m 12 | from cppflow.collision_detection import qpaths_batched_self_collisions, qpaths_batched_env_collisions 13 | 14 | K_JLIM_COST = 100 15 | K_COLLISION_COST = 1000 16 | 17 | 18 | # DEFAULT_JLIM_SAFETY_PADDING_REVOLUTE = np.deg2rad(1) # previous values 19 | # DEFAULT_JLIM_SAFETY_PADDING_PRISMATIC = cm_to_m(0.25) 20 | DEFAULT_JLIM_SAFETY_PADDING_REVOLUTE = np.deg2rad(1.5) 21 | DEFAULT_JLIM_SAFETY_PADDING_PRISMATIC = cm_to_m(3) # changed from 0.25 to 3 due to fetch__hello failures 22 | 23 | 24 | # NOTE: Really bad LM convergence when increasing eps past 2deg for 'fetch_arm__hello', 'fetch_arm__rot_yz' 25 | def joint_limit_almost_violations_3d( 26 | robot: Robot, 27 | qs: torch.Tensor, 28 | eps_revolute: float = DEFAULT_JLIM_SAFETY_PADDING_REVOLUTE, 29 | eps_prismatic: float = DEFAULT_JLIM_SAFETY_PADDING_PRISMATIC, 30 | ) -> torch.Tensor: 31 | """Returns a tensor with 1's where the joint angle is within eps of its limit and 0 otherwise 32 | 33 | Timing data: 34 | - fetch-circle: k=175 -> 0.001915, k=300 -> 0.002455 s 35 | 36 | Timing from the previous implementation: 37 | - fetch-circle: k=175 -> 0.0723s, k=300 -> 0.1263 s 38 | 39 | Args: 40 | qs (torch.Tensor): Should be [k x ntimesteps x n_dofs] 41 | 42 | Returns: 43 | torch.Tensor: [k x ntimesteps] 44 | """ 45 | assert len(qs.shape) == 3 46 | l_lim = torch.tensor([l for l, _ in robot.actuated_joints_limits], dtype=qs.dtype, device=qs.device) 47 | u_lim = torch.tensor([u for _, u in robot.actuated_joints_limits], dtype=qs.dtype, device=qs.device) 48 | l_lim[robot.prismatic_joint_idxs] += eps_prismatic 49 | l_lim[robot.revolute_joint_idxs] += eps_revolute 50 | u_lim[robot.prismatic_joint_idxs] -= eps_prismatic 51 | u_lim[robot.revolute_joint_idxs] -= eps_revolute 52 | return torch.logical_or((qs < l_lim).any(dim=2), (qs > u_lim).any(dim=2)).type(torch.float32) 53 | 54 | 55 | def dp_search_slow(problem: Problem, qpaths: List[torch.Tensor], use_cuda: bool = False, verbosity: int = 1): 56 | q = torch.stack(qpaths).detach().to(DEVICE) # [k x ntimesteps x n_dofs] 57 | k, ntimesteps, ndof = q.shape 58 | q_device = "cpu" if not use_cuda else DEVICE 59 | memo = torch.zeros((k, ntimesteps), dtype=torch.int32) 60 | 61 | with TimerContext("calculating configs near joint limits in dp_search()", enabled=verbosity > 0): 62 | jlimit_violations = joint_limit_almost_violations_3d(problem.robot, q).to(q_device) 63 | 64 | with TimerContext("calculating self-colliding configs in dp_search()", enabled=verbosity > 0): 65 | self_collision_violations = qpaths_batched_self_collisions(problem, q).to(q_device) 66 | 67 | with TimerContext("calculating env-colliding configs in dp_search()", enabled=verbosity > 0): 68 | env_collision_violations = qpaths_batched_env_collisions(problem, q).to(q_device) 69 | 70 | costs = torch.zeros((k, ntimesteps), device=q_device, dtype=q.dtype) 71 | q_costs_external = ( 72 | K_JLIM_COST * jlimit_violations 73 | + K_COLLISION_COST * env_collision_violations 74 | + K_COLLISION_COST * self_collision_violations 75 | ) 76 | 77 | costs[:, 0] = q_costs_external[:, 0] 78 | q = q.to(q_device) 79 | 80 | for t in range(1, ntimesteps): 81 | for ki in range(k): 82 | q_k_t = q[ki, t, :] 83 | dqs = q_k_t - q[:, t - 1, :] # [n_dofs] - [k x n_dofs] 84 | absdqs = torch.abs(torch.remainder(dqs + torch.pi, 2 * torch.pi) - torch.pi) 85 | maxdqs, _ = torch.max(absdqs, 1) 86 | t_next_cost = torch.maximum(maxdqs, costs[:, t - 1]) 87 | t_next_cost += q_costs_external[ki, t] 88 | costs[ki, t], memo[ki, t] = torch.min(t_next_cost, 0) 89 | 90 | # Extract best path 91 | best_path = torch.zeros((ntimesteps, ndof), dtype=q.dtype).to(q.device) 92 | _, i = torch.min(costs[:, -1], 0) 93 | for t in range(ntimesteps - 1, -1, -1): 94 | best_path[t, :] = q[i, t, :] 95 | i = memo[i, t] 96 | 97 | return best_path 98 | 99 | 100 | def _get_mjacs(q: torch.Tensor, robot: Robot, prismatic_joint_scaling: float = 5.0) -> torch.Tensor: 101 | """Returns a tensor of the maximum joint angle changes for each timestep from each of the k paths to one another. 102 | 103 | Args: 104 | q (torch.Tensor): [k, ntimesteps, ndof] tensor of joint angles 105 | prismatic_joint_scaling (float, optional): Scaling factor for change in value of the prismatic joints. Without 106 | this scaling term, q-deltas between prismatic and revolute joints 107 | are equally weighted, which means a change of 1 rad = 1 meter. This 108 | would make 57.2958 deg = 100cm -> 5 deg = 8.726cm. A value of 5.0 109 | was found to provide a good balance between decreasing the prismatic 110 | mjac while not increasing the revolute mjac too greatly. 111 | 112 | Returns: 113 | torch.Tensor: [k, k, ntimesteps - 1] tensor of mjacs 114 | """ 115 | k, ntimesteps, ndof = q.shape 116 | dqs = q[:, 1:, :].unsqueeze(1).expand(k, k, ntimesteps - 1, ndof) - q[:, :-1, :].unsqueeze(0).expand( 117 | k, k, ntimesteps - 1, ndof 118 | ) # [k, k, ntimesteps - 1, ndof] 119 | 120 | if robot.has_prismatic_joints: 121 | dqs[:, :, :, robot.prismatic_joint_idxs] *= prismatic_joint_scaling 122 | 123 | abs_dqs = torch.abs(torch.remainder(dqs + torch.pi, 2 * torch.pi) - torch.pi) # [k, k, ntimesteps - 1, ndof] 124 | mjacs, _ = torch.max(abs_dqs, 3) # [k, k, ntimesteps - 1] 125 | return mjacs 126 | 127 | 128 | def dp_search( 129 | robot: Robot, 130 | q: torch.Tensor, 131 | self_collision_violations: torch.Tensor, 132 | env_collision_violations: torch.Tensor, 133 | use_cuda: bool = False, 134 | verbosity: int = 1, 135 | ): 136 | """ 137 | q (torch.Tensor): [k x ntimesteps x n_dofs] tensor of joint configurations 138 | """ 139 | k, ntimesteps, ndof = q.shape 140 | q_device = "cpu" if not use_cuda else DEVICE 141 | q = q.to(q_device) 142 | 143 | with TimerContext("calculating configs near joint limits in dp_search()", enabled=verbosity > 0): 144 | jlimit_violations = joint_limit_almost_violations_3d(robot, q).to(q_device) 145 | costs = torch.zeros((k, ntimesteps), device=q_device, dtype=q.dtype) 146 | q_costs_external = ( 147 | K_JLIM_COST * jlimit_violations 148 | + K_COLLISION_COST * env_collision_violations.to(q_device) 149 | + K_COLLISION_COST * self_collision_violations.to(q_device) 150 | ) 151 | costs[:, 0] = q_costs_external[:, 0] 152 | 153 | # Calculate mjacs, run search 154 | mjacs = _get_mjacs(q, robot) 155 | memo = torch.zeros((k, ntimesteps), dtype=torch.int32) 156 | for t in range(1, ntimesteps): 157 | t_next_cost = torch.maximum(mjacs[:, :, t - 1], costs[:, t - 1]) # [k, k] 158 | t_next_cost += q_costs_external[:, t].unsqueeze(0).expand(k, k).transpose(0, 1) 159 | costs[:, t], memo[:, t] = torch.min(t_next_cost, 1) # [k] 160 | 161 | best_path = torch.zeros((ntimesteps, ndof), dtype=q.dtype).to(q.device) 162 | _, i = torch.min(costs[:, -1], 0) 163 | if verbosity > 1: 164 | xs = [] 165 | ys = [] 166 | 167 | for t in range(ntimesteps - 1, -1, -1): 168 | if verbosity > 1: 169 | xs.append(t) 170 | ys.append(i.item()) 171 | 172 | best_path[t, :] = q[i, t, :] 173 | i = memo[i, t] 174 | 175 | # Plot the path 176 | if verbosity > 2: 177 | warnings.warn("FYI: SAVING FIGURE. REMOVE THIS WHEN TIMING MATTERS") 178 | plt.figure(figsize=(10, 10)) 179 | plt.tight_layout() 180 | plt.imshow(q_costs_external, vmin=0, vmax=K_COLLISION_COST * 2 + K_JLIM_COST) 181 | plt.plot(xs, ys, label="best path", color="red") 182 | plt.title("dp_search() cost landscape and returned path") 183 | plt.xlabel("timestep") 184 | plt.ylabel("k") 185 | plt.legend() 186 | plt.colorbar() 187 | plt.grid(True, which="both", axis="both") 188 | plt.savefig("debug__dp_search_path.png", bbox_inches="tight") 189 | plt.close() 190 | 191 | return best_path 192 | -------------------------------------------------------------------------------- /cppflow/utils.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | from typing import Dict, List, Optional 3 | import os 4 | import random 5 | from time import time 6 | import colorsys 7 | 8 | import matplotlib.pyplot as plt 9 | import pkg_resources 10 | import torch 11 | import numpy as np 12 | 13 | from cppflow.config import DEVICE, DEFAULT_TORCH_DTYPE 14 | 15 | 16 | def print_v1(s, verbosity=0, *args, **kwargs): 17 | """Prints if verbsotity is 1 or greater""" 18 | if verbosity >= 1: 19 | print(s, *args, **kwargs) 20 | 21 | 22 | def print_v2(s, verbosity=0, *args, **kwargs): 23 | """Prints if verbsotity is 2 or greater""" 24 | if verbosity >= 2: 25 | print(s, *args, **kwargs) 26 | 27 | 28 | def print_v3(s, verbosity=0, *args, **kwargs): 29 | """Prints if verbsotity is 3 or greater""" 30 | if verbosity >= 3: 31 | print(s, *args, **kwargs) 32 | 33 | 34 | def _print_kwargs(kwargs, verbosity=0): 35 | if verbosity < 1: 36 | return 37 | for key, v in kwargs.items(): 38 | if key == "results_df" or key[0] == "_": 39 | continue 40 | if isinstance(v, tuple): 41 | print(f" {key}: (", end=" ") 42 | for vv in v: 43 | if isinstance(vv, torch.Tensor): 44 | print(vv.shape, end=", ") 45 | else: 46 | print(vv, end=", ") 47 | print(")") 48 | continue 49 | print(f" {key}: {v}") 50 | print() 51 | 52 | 53 | def _plot_self_collisions(self_collision_violations: torch.Tensor): 54 | plt.figure(figsize=(10, 10)) 55 | plt.tight_layout() 56 | plt.imshow(self_collision_violations.cpu().numpy(), vmin=0, vmax=1) 57 | plt.title("self collision violations") 58 | plt.xlabel("timestep") 59 | plt.ylabel("k") 60 | # plt.colorbar() 61 | plt.grid(True, which="both", axis="both") 62 | plt.savefig("debug__self_collision_violations.png", bbox_inches="tight") 63 | plt.close() 64 | 65 | 66 | def _plot_env_collisions(env_collision_violations: torch.Tensor): 67 | plt.figure(figsize=(10, 10)) 68 | plt.tight_layout() 69 | plt.imshow(env_collision_violations.cpu().numpy(), vmin=0, vmax=1) 70 | plt.title("environment collision violations") 71 | plt.xlabel("timestep") 72 | plt.ylabel("k") 73 | # plt.colorbar() 74 | plt.grid(True, which="both", axis="both") 75 | plt.savefig("debug__env_collision_violations.png", bbox_inches="tight") 76 | plt.close() 77 | 78 | 79 | TORM_TL_RESULTS = { 80 | "fetch_arm__circle": (11.105, None), 81 | "fetch_arm__hello": (61.029, None), 82 | "fetch_arm__rot_yz2": (29.913, None), 83 | "fetch_arm__s": (10.856, None), 84 | "fetch_arm__square": (14.841, None), 85 | "fetch__circle": (22.042, 0.498), 86 | "fetch__hello": (None, None), 87 | "fetch__rot_yz2": (30.419, 0.841), 88 | "fetch__s": (None, None), 89 | "fetch__square": (19.577, 0.56392), 90 | "panda__1cube": (8.493, None), 91 | "panda__2cubes": (12.628, None), 92 | "panda__flappy_bird": (None, None), 93 | } 94 | 95 | 96 | def get_TL_rad_torm_ratio(problem_name: str, tl_rad: Optional[float], tl_m: Optional[float]) -> Optional[float]: 97 | """Returns the ratio of trajectory length (TL) between the provided value and what torm reports""" 98 | tl_rad_torm = TORM_TL_RESULTS[problem_name][0] 99 | tl_m_torm = TORM_TL_RESULTS[problem_name][1] 100 | 101 | ratio_rev = None 102 | if tl_rad is not None and tl_rad_torm is not None: 103 | ratio_rev = tl_rad / tl_rad_torm 104 | 105 | ratio_pri = None 106 | if tl_m is not None and tl_m_torm is not None: 107 | ratio_pri = tl_m / tl_m_torm 108 | 109 | return ratio_rev, ratio_pri, tl_rad_torm, tl_m_torm 110 | 111 | 112 | class Hashable: 113 | def get_hash(self, ignore: List[str] = []) -> str: 114 | hash_str = "" 115 | for k, v in self.__dict__.items(): 116 | if k[0] == "_": 117 | continue 118 | if k in ignore: 119 | continue 120 | if isinstance(v, torch.Tensor): 121 | if v.numel() > 0: 122 | hash_str += f"{k}={v.sum().item()}+{v.shape}+{v.min().item()}+{v.max().item()}," 123 | else: 124 | hash_str += f"{k}=," 125 | continue 126 | hash_str += f"{k}={v}," 127 | return calc_hash(hash_str) 128 | 129 | 130 | class TimerContext: 131 | def __init__(self, name: str, enabled: bool = True, round_places: int = 4): 132 | self.name = name 133 | self.enabled = enabled 134 | self.round_places = round_places 135 | 136 | def __enter__(self): 137 | self.start = time() 138 | 139 | def __exit__(self, exception_type, exception_value, exception_traceback): 140 | if exception_type is not None: 141 | raise RuntimeError(f"Error caught by '{self.name}': {exception_value}") 142 | if self.enabled: 143 | print(f" --> {self.name} took {round(time() - self.start, self.round_places)} seconds") 144 | 145 | 146 | @dataclass 147 | class TestSpecification(Hashable): 148 | planner: str 149 | problem: str 150 | k: int 151 | n_runs: int 152 | 153 | def __post_init__(self): 154 | assert self.planner == "CppFlowPlanner" 155 | 156 | 157 | @dataclass 158 | class TestResult: 159 | planner: str 160 | problem: str 161 | k: int 162 | # success info 163 | succeeded: bool 164 | is_self_collision: bool 165 | is_env_collision: bool 166 | mjac_rev_invalid: bool 167 | mjac_pri_invalid: bool 168 | pose_pos_invalid: bool 169 | pose_rot_invalid: bool 170 | # runtime 171 | n_optimization_steps: int 172 | time_total: float 173 | time_ikflow: float 174 | time_dp_search: float 175 | time_optimizer: float 176 | # other 177 | search_path_mjac_deg: float 178 | search_path_mjac_cm: float 179 | search_path_min_dist_to_jlim_cm: float 180 | search_path_min_dist_to_jlim_deg: float 181 | tl_rad: float 182 | tl_m: float 183 | 184 | def __post_init__(self): 185 | assert self.planner in {"CppFlowPlanner"} 186 | 187 | 188 | def cm_to_m(x: float): 189 | return x / 100.0 190 | 191 | 192 | def get_filepath(local_filepath: str): 193 | return pkg_resources.resource_filename(__name__, local_filepath) 194 | 195 | 196 | def set_seed(seed: int = 0): 197 | torch.manual_seed(seed) 198 | torch.cuda.manual_seed_all(seed) 199 | torch.backends.cudnn.deterministic = True 200 | torch.backends.cudnn.benchmark = False 201 | np.random.seed(seed) 202 | random.seed(seed) 203 | os.environ["PYTHONHASHSEED"] = str(0) 204 | print("set_seed() - random int: ", torch.randint(0, 1000, (1, 1)).item()) 205 | 206 | 207 | def calc_hash(x: str, ignore_dict_keys: List = []) -> str: 208 | if isinstance(x, dict): 209 | hash_str = "" 210 | keys = sorted([str(k) for k in list(x.keys())]) 211 | for k in keys: 212 | if k in ignore_dict_keys: 213 | continue 214 | hash_str += f"{k}:{x[k]}" 215 | return calc_hash(hash_str) 216 | if isinstance(x, (int, float)): 217 | x = str(x) 218 | res = 0 219 | for ch in x: 220 | res = (res * 281 ^ ord(ch) * 997) & 0xFFFFFFFF 221 | return str(hex(res)[2:].lower().zfill(8))[0:8] 222 | 223 | 224 | def np_equal(a: np.ndarray, b: np.ndarray) -> bool: 225 | return (a.shape == b.shape) and (a == b).all() 226 | 227 | 228 | def np_hash(arr: np.ndarray) -> int: 229 | return hash(str(arr)) 230 | 231 | 232 | def to_torch(x: np.ndarray, device: str = DEVICE, dtype: torch.dtype = DEFAULT_TORCH_DTYPE) -> torch.Tensor: 233 | if isinstance(x, torch.Tensor): 234 | return x 235 | return torch.tensor(x, device=device, dtype=dtype) 236 | 237 | 238 | def to_numpy(x: torch.Tensor) -> np.ndarray: 239 | """Return a tensor/np array as a numpy array.""" 240 | if isinstance(x, torch.Tensor): 241 | return x.detach().cpu().numpy() 242 | return x 243 | 244 | 245 | def m_to_mm(x): 246 | return x * 1000.0 247 | 248 | 249 | def cm_to_mm(x): 250 | return x * 10.0 251 | 252 | 253 | def boolean_string(s): 254 | if isinstance(s, bool): 255 | return s 256 | if s.upper() not in {"FALSE", "TRUE"}: 257 | raise ValueError(f'input: "{s}" ("{type(s)}") is not a valid boolean string') 258 | return s.upper() == "TRUE" 259 | 260 | 261 | def non_private_dict(d: Dict) -> Dict: 262 | r = {} 263 | for k, v in d.items(): 264 | if k[0] == "_": 265 | continue 266 | r[k] = v 267 | return r 268 | 269 | 270 | def add_prefix_to_dict(d: Dict, prefix: str) -> Dict: 271 | d_ret = {} 272 | for k, v in d.items(): 273 | d_ret[f"{prefix}/{k}"] = v 274 | return d_ret 275 | 276 | 277 | def dict_subtraction(d: Dict, keys_to_exclude: set) -> Dict: 278 | return {k: v for k, v in d.items() if k not in keys_to_exclude} 279 | 280 | 281 | def assert_kwargs_is_valid(kwargs: Dict, allowed_keys: Dict): 282 | for key in kwargs: 283 | assert key in allowed_keys, f"Error, kwargs argument '{key}' not found in valid set {allowed_keys}" 284 | 285 | 286 | class bcolors: 287 | HEADER = "\033[95m" 288 | OKBLUE = "\033[94m" 289 | OKCYAN = "\033[96m" 290 | GREEN = "\033[92m" 291 | WARNING = "\033[93m" 292 | FAIL = "\033[91m" 293 | ENDC = "\033[0m" 294 | BOLD = "\033[1m" 295 | UNDERLINE = "\033[4m" 296 | 297 | 298 | def make_text_green_or_red(text: str, print_green: bool) -> str: 299 | if print_green: 300 | s = bcolors.GREEN 301 | else: 302 | s = bcolors.FAIL 303 | return s + str(text) + bcolors.ENDC 304 | 305 | 306 | def get_evenly_spaced_colors(n): 307 | base_color = 100 # Base color in HSL (green) 308 | colors = [] 309 | for i in range(n): 310 | hue = (base_color + (i * (360 / n))) % 360 # Adjust the step size (30 degrees in this case) 311 | rgb = colorsys.hsv_to_rgb(hue / 360, 1.0, 1.0) 312 | colors.append(rgb) 313 | return colors 314 | -------------------------------------------------------------------------------- /lint: -------------------------------------------------------------------------------- 1 | # Follow these two webpages to setup black to automatically run on saving 2 | # https://marcobelo.medium.com/setting-up-python-black-on-visual-studio-code-5318eba4cd00# 3 | # https://dev.to/adamlombard/vscode-setting-line-lengths-in-the-black-python-code-formatter-1g62 4 | 5 | echo "_____________" 6 | echo "Running ruff" 7 | echo "" 8 | uv run ruff check *.py --fix 9 | uv run ruff check **/*.py --fix 10 | uv run ruff check cppflow/ros2/*.py --fix 11 | 12 | echo "______________" 13 | echo "Running pylint" 14 | echo "" 15 | uv run python -m pylint --ignore cppflow/latent_optimization.py cppflow/*.py -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | def main(): 2 | print("Hello from cppflow-public!") 3 | 4 | 5 | if __name__ == "__main__": 6 | main() 7 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | cppflow 6 | 0.0.0 7 | Implementation of CppFlow - a fast cartesian planner 8 | Jeremy Morgan 9 | TODO: License declaration 10 | 11 | bdai_ros 12 | bdai_ros2_wrappers 13 | rclpy 14 | std_msgs 15 | cppflow_msgs 16 | 17 | 18 | 19 | rosidl_default_generators 20 | rosidl_default_runtime 21 | rosidl_interface_packages 22 | 23 | 24 | ament_python 25 | 26 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "cppflow" 3 | version = "0.1.0" 4 | description = "" 5 | requires-python = ">=3.10,<3.12" 6 | authors = [ 7 | { name="Jeremy Morgan", email="jsmorgan6@gmail.com" }, 8 | { name="David Millard", email="dmillard@gmail.com" } 9 | ] 10 | readme = "README.md" 11 | dependencies = [ 12 | "jrl @ git+https://github.com/jstmn/jrl.git@ef4c2f6eb1ba84395ff0bb01d5b7713854df6908", 13 | "ikflow @ git+https://github.com/jstmn/ikflow.git@19634d436eb805f22578b4c5b6526f96d91d0323", 14 | "psutil>=7.0.0", 15 | "ruff>=0.11.11", 16 | "black>=25.1.0", 17 | "pytest>=8.3.5", 18 | ] 19 | 20 | [tool.setuptools.packages] 21 | find = { include = ["cppflow"], exclude = ["media", "examples", "notebooks"] } 22 | 23 | [tool.setuptools] 24 | include-package-data = true 25 | 26 | [tool.setuptools.package-data] 27 | cppflow = ["**/*.yaml", "**/*.csv", "paths/paths_torm/"] 28 | 29 | 30 | 31 | [tool.ruff] 32 | line-length = 120 33 | lint.ignore = ["E741"] 34 | -------------------------------------------------------------------------------- /run_tests: -------------------------------------------------------------------------------- 1 | python -m unittest tests/*.py 2 | -------------------------------------------------------------------------------- /scripts/benchmark.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | import sys 3 | from datetime import datetime 4 | from io import StringIO 5 | from warnings import warn 6 | import argparse 7 | import os 8 | 9 | import torch 10 | import pandas as pd 11 | 12 | from cppflow.planners import PlannerSearcher, CppFlowPlanner 13 | from cppflow.data_types import ALL_PROBLEM_FILENAMES, get_problem_dict 14 | from cppflow.utils import set_seed 15 | from cppflow.config import SELF_COLLISIONS_IGNORED, ENV_COLLISIONS_IGNORED, DEBUG_MODE_ENABLED 16 | 17 | RESULTS_CSV_COLS = ( 18 | "Time Elapsed (s)", 19 | "is valid", 20 | "Mean Pos Error (mm)", 21 | "Max Pos Error (mm)", 22 | "Mean Rot Error (deg)", 23 | "Max Rot Error (deg)", 24 | "Mjac (deg)", 25 | "Mjac (cm)", 26 | "pct_self-colliding", 27 | "pct_env-colliding", 28 | "path_length_rad", 29 | "path_length_m", 30 | ) 31 | 32 | 33 | torch.set_printoptions(linewidth=120) 34 | set_seed() 35 | 36 | PLANNERS = { 37 | "CppFlowPlanner": CppFlowPlanner, 38 | "PlannerSearcher": PlannerSearcher, 39 | } 40 | 41 | 42 | def main(): 43 | n_files = len( 44 | [ 45 | item 46 | for item in os.listdir("scripts/benchmarking_output") 47 | if os.path.isfile(os.path.join("scripts/benchmarking_output", item)) 48 | ] 49 | ) 50 | assert n_files == 1, f"Expected 1 file in 'scripts/benchmarking_output/' but found {n_files}." 51 | assert SELF_COLLISIONS_IGNORED == ENV_COLLISIONS_IGNORED == DEBUG_MODE_ENABLED == False 52 | 53 | parser = argparse.ArgumentParser() 54 | parser.add_argument("--planner_name", type=str, required=True) 55 | args = parser.parse_args() 56 | planner_name = args.planner_name 57 | 58 | # 0.034671815s/step, 0.07943 s to log. 60s -> 1813 steps -> 1813*0.07943=2.4min for logging 59 | # --> 44min / rerun 60 | 61 | n_reruns = 10 62 | # n_reruns = 3 63 | # n_reruns = 1 64 | 65 | kwargs_dict = { 66 | "CppFlowPlanner": { 67 | "k": 175, 68 | "verbosity": 0, 69 | "run_batch_opt": False, 70 | "do_rerun_if_large_dp_search_mjac": True, 71 | "do_rerun_if_optimization_fails": True, 72 | # "only_1st": True, 73 | "only_1st": False, 74 | }, 75 | } 76 | assert not kwargs_dict[planner_name][ 77 | "only_1st" 78 | ], "'only_1st' returns after the first trajectory is found. you probably don't want to enable this" 79 | 80 | problems = ALL_PROBLEM_FILENAMES 81 | # problems = ["fetch_arm__s", "fetch_arm__circle"] 82 | # problems = ["fetch_arm__s"] 83 | # problems = ["fetch__s"] 84 | problem_dict = get_problem_dict(problems) 85 | 86 | for _, problem in problem_dict.items(): 87 | robot_name = problem.robot.name 88 | problem_name = problem.name 89 | for i in range(n_reruns): 90 | print("\n===========================================================================================") 91 | print(f" === {planner_name}\t| {robot_name} - {problem_name}\t{i+1}/{n_reruns} ===\n") 92 | problem = problem_dict[f"{robot_name}__{problem_name}"] 93 | planner = PLANNERS[planner_name](problem.robot) 94 | print(problem) 95 | results_df = {"df": pd.DataFrame(columns=RESULTS_CSV_COLS), "t0": time()} 96 | planner_result = planner.generate_plan(problem, **kwargs_dict[planner_name], results_df=results_df) 97 | print(planner_result.plan) 98 | 99 | # Save result to benchmarking_output/ 100 | results_filepath = ( 101 | f"scripts/benchmarking_output/cppflow__{planner_name}__{robot_name}__{problem_name}__results.csv" 102 | ) 103 | s = StringIO() 104 | results_df["df"].to_csv(s) 105 | with open(results_filepath, "a") as f: 106 | f.write("\n\n==========\n") 107 | f.write(s.getvalue()) 108 | f.write("==========\n") 109 | print(results_df["df"]) 110 | 111 | # Write kwargs as well for record keeping 112 | markdown_filepath = ( 113 | f"scripts/benchmarking_output/cppflow__{planner_name}__{datetime.now().strftime('%m.%d-%H:%M')}.md" 114 | ) 115 | with open(markdown_filepath, "w") as f: 116 | dt_str = datetime.now().strftime("%m.%d-%H:%M:%S") 117 | cli_input = "uv run python " + " ".join(sys.argv) 118 | f.write("# Planner results") 119 | f.write(f"\n\ndt: {dt_str} | cli_input: `{cli_input}`\n") 120 | f.write("\n\nparams:\n") 121 | for k, v in kwargs_dict.items(): 122 | f.write(f"- {k}: `{v}`\n") 123 | f.write("\n\n") 124 | 125 | 126 | """ 127 | uv run python scripts/benchmark.py --planner_name=CppFlowPlanner 128 | """ 129 | 130 | if __name__ == "__main__": 131 | warn("Ensure no other compute processes are running on the machine - this includes jupyter notebooks.") 132 | main() 133 | -------------------------------------------------------------------------------- /scripts/benchmarking_output/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jstmn/cppflow/c92e0a45aeee257a9a489b49cf83370193fe5844/scripts/benchmarking_output/.gitkeep -------------------------------------------------------------------------------- /scripts/create_path.py: -------------------------------------------------------------------------------- 1 | from typing import List, Tuple 2 | import argparse 3 | 4 | import pandas as pd 5 | import numpy as np 6 | from pyquaternion import Quaternion 7 | 8 | np.set_printoptions(suppress=True) 9 | 10 | paths = { 11 | # there are 25 interpolated poses between each waypoint in the original path 12 | "rot_yz2": [ 13 | ([0.0, 0.0, 0.0], [1.000000, 0.0, 0.0, 0.0], 30), 14 | ([0.0, 0.0, 0.0], [0.923880, 0.0, 0.382683, 0.0], 30), # rot_y(45 deg) 15 | ([0.0, 0.0, 0.0], [1.000000, 0.0, 0.0, 0.0], 30), 16 | ([0.0, 0.0, 0.0], [0.923880, 0.0, -0.382683, 0.0], 30), 17 | ([0.0, 0.0, 0.0], [1.000000, 0.0, 0.0, 0.0], 30), 18 | ([0.0, 0.0, 0.0], [0.923880, 0.0, 0.0, 0.382683], 30), 19 | ([0.0, 0.0, 0.0], [1.000000, 0.0, 0.0, 0.0], 30), 20 | ([0.0, 0.0, 0.0], [0.923880, 0.0, 0.0, -0.382683], 30), 21 | ([0.0, 0.0, 0.0], [1.000000, 0.0, 0.0, 0.0], 30), 22 | ] 23 | } 24 | 25 | 26 | def circle_path(output_filepath: str): 27 | # Circle centered at (0, -0.25, 0) with radius 0.25 in the yz plane 28 | n_waypoints = 500 29 | xs = np.zeros(n_waypoints) 30 | thetas = np.linspace(0, 2 * np.pi, n_waypoints) 31 | ys = -0.25 + 0.25 * np.cos(thetas) 32 | zs = 0.25 * np.sin(thetas) 33 | 34 | time_out = np.arange(n_waypoints).reshape((n_waypoints, 1)) 35 | rotations_out = np.zeros((n_waypoints, 4)) 36 | rotations_out[:, 0] = 1.0 37 | poses_out = np.hstack( 38 | ( 39 | time_out, 40 | xs.reshape((n_waypoints, 1)), 41 | ys.reshape((n_waypoints, 1)), 42 | zs.reshape((n_waypoints, 1)), 43 | rotations_out, 44 | ) 45 | ) 46 | 47 | # save result to a csv 48 | df = pd.DataFrame(poses_out, columns=["time", "x", "y", "z", "qw", "qx", "qy", "qz"]) 49 | df.to_csv(output_filepath, index=False) 50 | 51 | 52 | def path_from_spaced_waypoints(waypoints: List[Tuple[List, List]], output_filepath: str): 53 | positions_in = np.array([wp[0] for wp in waypoints]) 54 | assert np.absolute(positions_in).max() < 1e-8, "positions must be 0 - non zero handling unimplemented" 55 | rotations_in = np.array([wp[1] for wp in waypoints]) 56 | 57 | rotations_out = [] 58 | 59 | for i in range(len(rotations_in) - 1): 60 | q0 = Quaternion(waypoints[i][1]) 61 | qf = Quaternion(waypoints[i + 1][1]) 62 | rotations_out.append([q0.w, q0.x, q0.y, q0.z]) 63 | for q in Quaternion.intermediates(q0, qf, waypoints[i][2], include_endpoints=False): 64 | rotations_out.append([q.w, q.x, q.y, q.z]) 65 | # timestamps_in = np.array([0, 1]) 66 | # timestamps_out = np.linspace(0, 1, waypoint_in_0[2]) 67 | # xs = np.interp(timestamps_out, timestamps_in, [waypoint_in_0[0][0], waypoint_in_f[0][0]]) 68 | # ys = np.interp(timestamps_out, timestamps_in, [waypoint_in_0[0][0], waypoint_in_f[0][0]]) 69 | # zs = np.interp(timestamps_out, timestamps_in, [waypoint_in_0[0][0], waypoint_in_f[0][0]]) 70 | 71 | q_last = Quaternion(waypoints[-1][1]) 72 | rotations_out.append([q_last.w, q_last.x, q_last.y, q_last.z]) 73 | 74 | # format result 75 | n_poses_out = len(rotations_out) 76 | time_out = np.arange(n_poses_out).reshape((n_poses_out, 1)) 77 | positions_out = np.zeros((n_poses_out, 3)) 78 | rotations_out = np.array(rotations_out) 79 | poses_out = np.hstack((time_out, positions_out, rotations_out)) 80 | 81 | # save result to a csv 82 | df = pd.DataFrame(poses_out, columns=["time", "x", "y", "z", "qw", "qx", "qy", "qz"]) 83 | df.to_csv(output_filepath, index=False) 84 | 85 | 86 | """ Example usage 87 | 88 | uv run python scripts/create_path.py rot_yz2 rot_yz2.csv 89 | uv run python scripts/create_path.py circle2 circle2.csv 90 | """ 91 | 92 | if __name__ == "__main__": 93 | parser = argparse.ArgumentParser(description="Create a path") 94 | parser.add_argument("path_name", type=str) 95 | parser.add_argument("output_filepath", type=str) 96 | args = parser.parse_args() 97 | if args.path_name == "rot_yz2": 98 | path_from_spaced_waypoints(paths[args.path_name], args.output_filepath) 99 | elif args.path_name == "circle2": 100 | circle_path(args.output_filepath) 101 | -------------------------------------------------------------------------------- /scripts/evaluate.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, Type 2 | import argparse 3 | import sys 4 | from datetime import datetime 5 | import multiprocessing 6 | import os 7 | import socket 8 | import psutil 9 | 10 | import torch 11 | import pandas as pd 12 | 13 | from cppflow.planners import PlannerSearcher, CppFlowPlanner, Planner 14 | from cppflow.data_types import PlannerResult, TimingData, PlannerSettings, Constraints, Problem 15 | from cppflow.data_type_utils import problem_from_filename 16 | from cppflow.utils import set_seed, to_torch 17 | from cppflow.config import DEVICE, SELF_COLLISIONS_IGNORED, ENV_COLLISIONS_IGNORED, DEBUG_MODE_ENABLED 18 | from cppflow.visualization import visualize_plan, plot_plan 19 | from cppflow.collision_detection import qpaths_batched_self_collisions, qpaths_batched_env_collisions 20 | 21 | torch.set_printoptions(linewidth=120) 22 | set_seed() 23 | 24 | PLANNERS = { 25 | "CppFlow": CppFlowPlanner, 26 | "PlannerSearcher": PlannerSearcher, 27 | } 28 | 29 | BENCHMARKING_DIR = "/home/jstm/Projects/cppflowpaper2/benchmarking/cppflow" 30 | 31 | PD_COLUMN_NAMES = [ 32 | "Problem", 33 | "Robot", 34 | "Planner", 35 | "Valid plan", 36 | "time, total (s)", 37 | "time, ikflow (s)", 38 | "time, coll_checking (s)", 39 | "time, batch_opt (s)", 40 | "time, dp_search (s)", 41 | "time, optimizer (s)", 42 | "time per opt. step (s)", 43 | "Max positional error (mm)", 44 | "Max rotational error (deg)", 45 | "Mean positional error (mm)", 46 | "Mean rotational error (deg)", 47 | "Mjac - prismatic (cm)", 48 | "Mjac - revolute (deg)", 49 | ] 50 | 51 | CONSTRAINTS = Constraints( 52 | max_allowed_position_error_cm=0.01, # 0.1mm 53 | max_allowed_rotation_error_deg=0.1, 54 | max_allowed_mjac_deg=7.0, # from the paper 55 | max_allowed_mjac_cm=2.0, # from the paper 56 | ) 57 | 58 | 59 | def _eval_planner_on_problem(planner: Type[Planner], problem: Problem, planner_settings: PlannerSettings): 60 | result = planner.generate_plan(problem, planner_settings) 61 | print() 62 | print(result.plan) 63 | print() 64 | print(result.timing) 65 | time_per_optimization_step = result.timing.optimizer / result.debug_info["n_optimization_steps"] 66 | round_amt = 5 67 | return [ 68 | problem.fancy_name, 69 | problem.robot.name, 70 | planner.name, 71 | f"`{str(result.plan.is_valid).lower()}`", 72 | round(result.timing.total, 4), 73 | round(result.timing.ikflow, 4), 74 | round(result.timing.coll_checking, 4), 75 | round(result.timing.batch_opt, 4), 76 | round(result.timing.dp_search, 4), 77 | round(result.timing.optimizer, 4), 78 | round(time_per_optimization_step, 4), 79 | round(result.plan.max_positional_error_mm, round_amt), 80 | round(result.plan.mean_rotational_error_deg, round_amt), 81 | round(result.plan.mean_positional_error_mm, round_amt), 82 | round(result.plan.max_rotational_error_deg, round_amt), 83 | round(result.plan.mjac_cm, round_amt), 84 | round(result.plan.mjac_deg, round_amt), 85 | ], result.plan.is_valid 86 | 87 | 88 | # TODO: update to only use LmFull. Or move this to a new script that's just for saving data to the paper repo 89 | def eval_planners_on_problem(settings_dict: Dict, save_to_benchmarking: bool = True): 90 | """Run all planners on each problem""" 91 | # problems = get_all_problems() 92 | problems = [problem_from_filename("fetch_arm__square")] 93 | planner_clcs = [CppFlowPlanner] 94 | df_all = pd.DataFrame(columns=PD_COLUMN_NAMES) 95 | print("\n---------------------------------") 96 | for i, problem in enumerate(problems): 97 | print(f"\n\n{i} ======================\n") 98 | print(problem) 99 | 100 | df = pd.DataFrame(columns=PD_COLUMN_NAMES) 101 | 102 | for planner_clc in planner_clcs: 103 | planner = planner_clc(problem.robot) 104 | print("\n ======\n") 105 | print(planner) 106 | new_row = _eval_planner_on_problem(planner, problem, settings_dict[planner.name]) 107 | df_all.loc[len(df_all)] = new_row 108 | 109 | if save_to_benchmarking: 110 | assert psutil.cpu_count() == multiprocessing.cpu_count() 111 | now_str = datetime.now().strftime("%m.%d-%H:%M") 112 | df_all.to_csv(os.path.join(BENCHMARKING_DIR, f"results__{now_str}.csv")) 113 | with open(os.path.join(BENCHMARKING_DIR, f"results__{now_str}__params.md"), "a") as f: 114 | cli_input = "uv run python " + " ".join(sys.argv) 115 | f.write("# Parameters") 116 | f.write(f"\n\ndt: {now_str} | cli_input: `{cli_input}`\n") 117 | f.write("\n\nparams:\n") 118 | for k, v in settings_dict.__dict__.items(): 119 | if k[0] == "_": 120 | continue 121 | f.write(f"- {k}: `{v}`\n") 122 | f.write("\n\n") 123 | f.write("\n\ncomputer:\n") 124 | f.write(f"- hostname: `{socket.gethostname()}`\n") 125 | f.write(f"- gpu: `{torch.cuda.get_device_name(device=DEVICE)}`\n") 126 | f.write(f"- #cpus: `{multiprocessing.cpu_count()}`\n") 127 | f.write(f"- ram size: `{psutil.virtual_memory().total / (1024*1024*1024)}` gb\n") 128 | f.write("\n\n") 129 | 130 | 131 | def eval_planner_on_problems(planner_name: str, planner_settings: PlannerSettings): 132 | """Evaluate a planner on the given problems""" 133 | # problems = get_all_problems() 134 | problems = [ 135 | problem_from_filename("panda__1cube"), 136 | problem_from_filename("fetch_arm__square"), 137 | problem_from_filename("fetch__square"), 138 | ] 139 | planners = [PLANNERS[planner_name](problem.robot) for problem in problems] 140 | 141 | df = pd.DataFrame(columns=PD_COLUMN_NAMES) 142 | succeeded = [] 143 | failed = [] 144 | 145 | for problem, planner in zip(problems, planners): 146 | print("\n---------------------------------") 147 | print(problem) 148 | new_row, is_valid = _eval_planner_on_problem(planner, problem, planner_settings) 149 | assert len(new_row) == len(PD_COLUMN_NAMES), ( 150 | f"len(new_row)={len(new_row)} != len(PD_COLUMN_NAMES)={len(PD_COLUMN_NAMES)}\n. Column:" 151 | f" {PD_COLUMN_NAMES}\nrow: {new_row}" 152 | ) 153 | df.loc[len(df)] = new_row 154 | 155 | # Update valid_dict 156 | if is_valid: 157 | succeeded.append(problem.full_name) 158 | else: 159 | failed.append(problem.full_name) 160 | 161 | df = df.sort_values(by=["Robot", "Problem"]) 162 | df = df.drop(["Planner"], axis=1) 163 | 164 | dt = datetime.now().strftime("%d.%H:%M") 165 | with open(f"PLANNER_RESULTS - {planner_name} - {dt}.md", "w") as f: 166 | cli_input = "uv run python " + " ".join(sys.argv) 167 | f.write(f"\n\n**{dt}** | Generated with `{cli_input}`") 168 | f.write(f"\n\nPlanner: **{planner.name}**") 169 | f.write(f"\n\nparams: `{planner_settings}`\n\n") 170 | f.write(df.to_markdown()) 171 | 172 | valid_text = "\n```\n" 173 | valid_text += f"succeeded: {sorted(succeeded)}\n" 174 | valid_text += f"failed: {sorted(failed)}\n" 175 | valid_text += "```" 176 | f.write(valid_text) 177 | 178 | # df_success 179 | f.write("\n\n**Successful plans**:\n\n") 180 | df_success = df[df["Valid plan"] != "`false`"].copy() 181 | df_success = df_success.drop( 182 | [ 183 | "Valid plan", 184 | "Max positional error (mm)", 185 | "Max rotational error (deg)", 186 | "Mean positional error (mm)", 187 | "Mean rotational error (deg)", 188 | ], 189 | axis=1, 190 | ) 191 | f.write(df_success.to_markdown()) 192 | 193 | # df_failed 194 | f.write("\n\n**Failed plans**:\n\n") 195 | df_failed = df[df["Valid plan"] == "`false`"].copy() 196 | df_failed = df_failed.drop(["Valid plan"], axis=1) 197 | f.write(df_failed.to_markdown()) 198 | 199 | print(df) 200 | 201 | 202 | def get_initial_configuration(problem: Problem): 203 | for _ in range(25): 204 | initial_configuration = to_torch( 205 | problem.robot.inverse_kinematics_klampt(problem.target_path[0].cpu().numpy()) 206 | ).view(1, 1, problem.robot.ndof) 207 | if not ( 208 | qpaths_batched_env_collisions(problem, initial_configuration) 209 | or qpaths_batched_self_collisions(problem, initial_configuration) 210 | ): 211 | print(f"Initial configuration {initial_configuration} is collision free") 212 | return initial_configuration.view(1, problem.robot.ndof) 213 | raise RuntimeError("Could not find collision free initial configuration") 214 | 215 | 216 | """ 217 | 218 | Problems: 219 | - fetch_arm__circle 220 | - fetch_arm__hello 221 | - fetch_arm__rot_yz 222 | - fetch_arm__s 223 | - fetch_arm__square 224 | - fetch__circle 225 | - fetch__hello 226 | - fetch__rot_yz 227 | - fetch__s 228 | - fetch__square 229 | - panda__flappy_bird 230 | - panda__2cubes 231 | - panda__1cube 232 | 233 | Example usage: 234 | 235 | uv run python scripts/evaluate.py --all_1 --planner CppFlow 236 | uv run python scripts/evaluate.py --all_2 --save_to_benchmarking 237 | 238 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__circle --visualize 239 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__hello --visualize 240 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__rot_yz --visualize 241 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__rot_yz2 --visualize 242 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__s --visualize 243 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__square --visualize 244 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__circle --visualize 245 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__hello --visualize 246 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__rot_yz --visualize 247 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__rot_yz2 --visualize 248 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__s --visualize 249 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch__square --visualize 250 | uv run python scripts/evaluate.py --planner CppFlow --problem=panda__1cube --visualize 251 | uv run python scripts/evaluate.py --planner CppFlow --problem=panda__2cubes --visualize 252 | uv run python scripts/evaluate.py --planner CppFlow --problem=panda__flappy_bird --visualize 253 | 254 | uv run python scripts/evaluate.py --planner CppFlow --problem=fetch_arm__hello_mini --visualize --use_fixed_initial_configuration 255 | 256 | uv run python scripts/evaluate.py --planner CppFlow --problem=panda__1cube_mini --plan_filepath=many_env_collisions[0].pt 257 | uv run python scripts/evaluate.py --planner CppFlow --problem=panda__1cube_mini --plot --use_fixed_initial_configuration 258 | uv run python scripts/evaluate.py --planner CppFlow --problem=panda__1cube_mini 259 | """ 260 | 261 | 262 | def main(args): 263 | planner_settings_dict = { 264 | "CppFlow": PlannerSettings( 265 | verbosity=2, 266 | k=175, 267 | tmax_sec=5.0, 268 | anytime_mode_enabled=False, 269 | do_rerun_if_large_dp_search_mjac=True, 270 | do_rerun_if_optimization_fails=False, 271 | do_return_search_path_mjac=False, 272 | ), 273 | "CppFlow_fixed_q0": PlannerSettings( 274 | verbosity=2, 275 | k=175, 276 | tmax_sec=3.0, 277 | anytime_mode_enabled=False, 278 | latent_vector_scale=0.5, 279 | do_rerun_if_large_dp_search_mjac=False, 280 | do_rerun_if_optimization_fails=False, 281 | do_return_search_path_mjac=False, 282 | ), 283 | "PlannerSearcher": PlannerSettings( 284 | k=175, 285 | tmax_sec=5.0, 286 | anytime_mode_enabled=False, 287 | ), 288 | } 289 | planner_settings = ( 290 | planner_settings_dict[args.planner_name] 291 | if not args.use_fixed_initial_configuration 292 | else planner_settings_dict[args.planner_name + "_fixed_q0"] 293 | ) 294 | 295 | if args.problem is not None: 296 | problem = problem_from_filename(CONSTRAINTS, args.problem) 297 | print(problem) 298 | print(problem.constraints) 299 | 300 | if args.use_fixed_initial_configuration: 301 | problem.initial_configuration = get_initial_configuration() 302 | 303 | if args.plan_filepath is not None: 304 | plan = torch.load(args.plan_filepath) 305 | planner_result = PlannerResult(plan, TimingData(0, 0, 0, 0, 0, 0), [], [], {}) 306 | else: 307 | planner: Planner = PLANNERS[args.planner_name](planner_settings, problem.robot) 308 | planner_result = planner.generate_plan(problem) 309 | 310 | # save results to disk 311 | # torch.save(planner_result.plan, f"pt_tensors/plan__{problem.full_name}__{planner.name}.pt") 312 | # df = pd.DataFrame(planner_result.plan.q_path.cpu().numpy()) 313 | # df.to_csv(f"pt_tensors/plan__{problem.full_name}__{planner.name}.csv", index=False) 314 | 315 | print() 316 | print(" ====== Planner result ======") 317 | print() 318 | print(planner_result.plan) 319 | print() 320 | print(planner_result.timing) 321 | print() 322 | print("debug_info:") 323 | for k, v in planner_result.debug_info.items(): 324 | print(f" {k}: {v}") 325 | 326 | if args.plot: 327 | plot_plan(planner_result.plan, problem, planner_result.other_plans, planner_result.other_plans_names) 328 | if args.visualize: 329 | visualize_plan(planner_result.plan, problem, start_delay=3) 330 | 331 | if args.all_1: 332 | eval_planner_on_problems(args.planner_name, planner_settings) 333 | 334 | elif args.all_2: 335 | eval_planners_on_problem(planner_settings_dict, args.save_to_benchmarking) 336 | 337 | 338 | if __name__ == "__main__": 339 | assert SELF_COLLISIONS_IGNORED == ENV_COLLISIONS_IGNORED == DEBUG_MODE_ENABLED == False 340 | 341 | parser = argparse.ArgumentParser() 342 | parser.add_argument("--planner_name", type=str) 343 | parser.add_argument("--problem", type=str) 344 | parser.add_argument("--visualize", action="store_true") 345 | parser.add_argument("--plan_filepath", type=str) 346 | parser.add_argument("--plot", action="store_true") 347 | parser.add_argument("--all_1", action="store_true") 348 | parser.add_argument("--all_2", action="store_true") 349 | parser.add_argument("--save_to_benchmarking", action="store_true") 350 | parser.add_argument("--use_fixed_initial_configuration", action="store_true") 351 | args = parser.parse_args() 352 | 353 | main(args) 354 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/cppflow 3 | [install] 4 | install_scripts=$base/lib/cppflow -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from setuptools import setup, find_packages 3 | 4 | with open(os.path.join(os.path.dirname(__file__), "README.md"), "r") as fh: 5 | long_description = fh.read() 6 | 7 | package_name = "cppflow" 8 | 9 | setup( 10 | name=package_name, 11 | version="0.1.0", 12 | description="", 13 | author="Jeremy Morgan, David Millard", 14 | author_email="jsmorgan6@gmail.com, dmillard@gmail.com", 15 | license="MIT", 16 | long_description_content_type="text/markdown", 17 | long_description=long_description, 18 | python_requires=">=3.8.0,<3.11", 19 | packages=find_packages(), 20 | install_requires=[ 21 | "psutil==5.9.8", 22 | ], 23 | data_files=[ 24 | ("share/ament_index/resource_index/packages", ["cppflow/ros2/cppflow"]), 25 | ( 26 | f"share/{package_name}", 27 | ["package.xml", "README.md"], 28 | ), # Installs package.xml and README.md to /workspaces/bdai/_build/install/cppflow/share/cppflow 29 | ], 30 | entry_points={ 31 | "console_scripts": [ 32 | "ros2_subscriber = cppflow.ros2.ros2_subscriber:main", 33 | "ros2_publisher = cppflow.ros2.ros2_publisher:main", 34 | ], 35 | }, 36 | dependency_links=[ 37 | "git+https://github.com/jstmn/jrl.git@master#egg=jrl", 38 | "git+https://github.com/jstmn/ikflow.git@master#egg=ikflow", 39 | ], 40 | extras_require={ 41 | "dev": [ 42 | "black==23.1.0", 43 | "pylint==2.16.2", 44 | "pandas==1.5.3", 45 | "tabulate==0.9.0", 46 | "pytest==8.2.0", 47 | ] 48 | }, 49 | ) 50 | -------------------------------------------------------------------------------- /tests/collision_checking_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import torch 4 | 5 | from cppflow.utils import set_seed, to_torch 6 | from cppflow.problem import problem_from_filename, Problem 7 | from cppflow.collision_detection import ( 8 | get_only_non_colliding_qpaths, 9 | qpaths_batched_env_collisions, 10 | qpaths_batched_self_collisions, 11 | self_colliding_configs_capsule, 12 | env_colliding_configs_capsule, 13 | ) 14 | 15 | set_seed() 16 | 17 | 18 | def _only_non_colliding_gt(problem: Problem, qpaths: torch.Tensor): 19 | non_self_colliding = [qp for qp in qpaths if not self_colliding_configs_capsule(problem, qp).any()] 20 | print("# non-self_colliding: ", len(non_self_colliding), "/", len(qpaths)) 21 | safe = [qp for qp in non_self_colliding if not env_colliding_configs_capsule(problem, qp).any()] 22 | print("# safe: ", len(safe), "/", len(qpaths)) 23 | return safe 24 | 25 | 26 | class CollisionCheckingTest(unittest.TestCase): 27 | def test_qpaths_filtered_the_same(self): 28 | n = 5 29 | k = 50 30 | p_old = problem_from_filename("panda__1cube") 31 | problem = Problem( 32 | target_path=p_old.target_path[0:n], 33 | robot=p_old.robot, 34 | name=p_old.name, 35 | full_name=p_old.full_name, 36 | obstacles=p_old.obstacles, 37 | obstacles_Tcuboids=p_old.obstacles_Tcuboids, 38 | obstacles_cuboids=p_old.obstacles_cuboids, 39 | obstacles_klampt=p_old.obstacles_klampt, 40 | ) 41 | ikflow_qpaths = [to_torch(problem.robot.sample_joint_angles(n)) for _ in range(k)] 42 | 43 | # Ground truth 44 | filtered_gt = _only_non_colliding_gt(problem, ikflow_qpaths) 45 | assert len(filtered_gt) > 0 46 | 47 | # Code to test 48 | q = torch.stack(ikflow_qpaths) 49 | self_collision_violations = qpaths_batched_self_collisions(problem, q) 50 | env_collision_violations = qpaths_batched_env_collisions(problem, q) 51 | returned = get_only_non_colliding_qpaths(ikflow_qpaths, self_collision_violations, env_collision_violations) 52 | 53 | # Check 54 | self.assertEqual(len(filtered_gt), len(returned)) 55 | for qp1, qp2 in zip(filtered_gt, returned): 56 | self.assertTrue(torch.allclose(qp1, qp2)) 57 | 58 | 59 | if __name__ == "__main__": 60 | unittest.main() 61 | -------------------------------------------------------------------------------- /tests/evaluation_utils_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import torch 4 | from cppflow.evaluation_utils import angular_changes, calculate_per_timestep_mjac_deg, calculate_mjac_deg 5 | 6 | _2PI = 2 * torch.pi 7 | torch.set_default_dtype(torch.float32) 8 | torch.set_default_device("cpu") 9 | 10 | 11 | class EvaluationTest(unittest.TestCase): 12 | def test_mjac_consistency(self): 13 | qpath = torch.randn((10, 3)) 14 | self.assertAlmostEqual(calculate_mjac_deg(qpath), calculate_per_timestep_mjac_deg(qpath).max()) 15 | self.assertAlmostEqual(calculate_mjac_deg(qpath), torch.rad2deg(angular_changes(qpath).abs().max())) 16 | 17 | def test_angular_changes(self): 18 | """Test that the angular_changes() function returns the correct values.""" 19 | qpath = torch.tensor( 20 | [ 21 | [0, 0, 0], 22 | [0, 0, 0], 23 | [0, 0, 0], 24 | ], 25 | dtype=torch.float32, 26 | ) 27 | expected = torch.tensor( 28 | [ 29 | [0, 0, 0], 30 | [0, 0, 0], 31 | ], 32 | dtype=torch.float32, 33 | ) 34 | torch.testing.assert_close(expected, angular_changes(qpath)) 35 | 36 | # 37 | qpath = torch.tensor( 38 | [ 39 | [0, 0, 0], 40 | [0, 0, 0], 41 | [0, 0, 0.1], 42 | ], 43 | dtype=torch.float32, 44 | ) 45 | expected = torch.tensor( 46 | [ 47 | [0, 0, 0], 48 | [0, 0, 0.1], 49 | ], 50 | dtype=torch.float32, 51 | ) 52 | torch.testing.assert_close(expected, angular_changes(qpath)) 53 | 54 | # 55 | qpath = torch.tensor( 56 | [ 57 | [0, 0, 0], 58 | [0, 0, 0.1], 59 | [0, 0, -0.1], 60 | ], 61 | dtype=torch.float32, 62 | ) 63 | expected = torch.tensor( 64 | [ 65 | [0, 0, 0.1], 66 | [0, 0, -0.2], 67 | ], 68 | dtype=torch.float32, 69 | ) 70 | torch.testing.assert_close(expected, angular_changes(qpath)) 71 | 72 | # 73 | qpath = torch.tensor( 74 | [ 75 | [0, -0.05, 0], 76 | [0, 0, 0.1], 77 | [0, 0, -0.1], 78 | ], 79 | dtype=torch.float32, 80 | ) 81 | expected = torch.tensor( 82 | [ 83 | [0, 0.05, 0.1], 84 | [0, 0, -0.2], 85 | ], 86 | dtype=torch.float32, 87 | ) 88 | torch.testing.assert_close(expected, angular_changes(qpath)) 89 | 90 | # 91 | qpath = torch.tensor( 92 | [ 93 | [0, 0, 0], 94 | [0, 0, _2PI - 0.1], 95 | [0, 0, 0], 96 | ], 97 | dtype=torch.float32, 98 | ) 99 | expected = torch.tensor( 100 | [ 101 | [0, 0, -0.1], 102 | [0, 0, 0.1], 103 | ], 104 | dtype=torch.float32, 105 | ) 106 | torch.testing.assert_close(expected, angular_changes(qpath)) 107 | 108 | # 109 | qpath = torch.tensor( 110 | [ 111 | [0, 0, 0], 112 | [0, 0, _2PI - 0.1], 113 | [-0.5, 0, 0.2], 114 | ], 115 | dtype=torch.float32, 116 | ) 117 | expected = torch.tensor( 118 | [ 119 | [0, 0, -0.1], 120 | [-0.5, 0, 0.3], 121 | ], 122 | dtype=torch.float32, 123 | ) 124 | torch.testing.assert_close(expected, angular_changes(qpath)) 125 | 126 | 127 | if __name__ == "__main__": 128 | unittest.main() 129 | -------------------------------------------------------------------------------- /tests/fetch__s__truncated.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch" 2 | path_name: "s_truncated" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [1.0, 0.3, 0.55] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] -------------------------------------------------------------------------------- /tests/fetch_arm__s__truncated.yaml: -------------------------------------------------------------------------------- 1 | robot: "fetch_arm" 2 | path_name: "s_truncated" 3 | 4 | path_offset_frame: "torso_lift_link" 5 | path_xyz_offset: [1.0, 0.3, 0.55] 6 | path_R_offset: [ 7 | [1, 0, 0], 8 | [0, 1, 0], 9 | [0, 0, 1] 10 | ] -------------------------------------------------------------------------------- /tests/optimization_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from time import time 3 | 4 | import numpy as np 5 | import torch 6 | 7 | from cppflow.problem import problem_from_filename 8 | from cppflow.utils import set_seed, to_torch, make_text_green_or_red 9 | from cppflow.optimization import ( 10 | OptimizationParameters, 11 | OptimizationProblem, 12 | OptimizationState, 13 | levenberg_marquardt_full, 14 | levenberg_marquardt_only_pose, 15 | ) 16 | from cppflow.data_types import PlannerSearcher 17 | from cppflow.optimization_utils import LmResidualFns 18 | 19 | PI = torch.pi 20 | torch.set_printoptions(linewidth=5000, precision=8, sci_mode=False) 21 | np.set_printoptions(suppress=True, linewidth=120) 22 | set_seed() 23 | 24 | 25 | def levenberg_marquardt_full_naive( 26 | opt_problem: OptimizationProblem, 27 | opt_state: OptimizationState, 28 | opt_params: OptimizationParameters, 29 | lambd: float, 30 | return_residual: bool = False, 31 | ): 32 | """Calculate an update to x using the levenberg marquardt optimization procedure. Includes residual terms for 33 | minimizing the difference between sequential configs. 34 | """ 35 | assert opt_problem.parallel_count == 1 36 | assert opt_problem.robot.name != "fetch", "fetch unsupported. need to deal with prismatic joint first" 37 | n = opt_state.x.shape[0] 38 | ndof = opt_problem.robot.ndof 39 | # --- 40 | jacobian, residual = LmResidualFns.get_r_and_J( 41 | opt_params, 42 | opt_problem.robot, 43 | opt_state.x, 44 | opt_problem.target_path, 45 | Tcuboids=opt_problem.problem.obstacles_Tcuboids, 46 | cuboids=opt_problem.problem.obstacles_cuboids, 47 | ) 48 | J = jacobian.get_J() 49 | r = residual.get_r() 50 | assert r.shape[0] == J.shape[0], f"Shape error. r: {r.shape}, J: {J.shape}" 51 | # Solve (J^T*J + lambd*I)*delta_X = J^T*r 52 | # Naive solution - create giant, sparse matrix. Cholesky decomposition should reduce solve time 53 | J_T = torch.transpose(J, 0, 1) # 54 | eye = torch.eye(n * ndof, dtype=opt_state.x.dtype, device=opt_state.x.device) 55 | lhs = torch.matmul(J_T, J) + lambd * eye # [n*ndof x n*ndof] 56 | rhs = torch.matmul(J_T, r) # [n ndof 1] 57 | delta_x = torch.linalg.solve(lhs, rhs) # [n ndof 1] 58 | delta_x = delta_x.reshape((n, ndof)) 59 | if return_residual: 60 | return opt_state.x + delta_x, J, r 61 | return opt_state.x + delta_x 62 | 63 | 64 | class OptimizationTest(unittest.TestCase): 65 | @classmethod 66 | def setUpClass(cls): 67 | cls.fetch_arm__square = problem_from_filename("fetch_arm__square") 68 | cls.fetch_arm = cls.fetch_arm__square.robot 69 | 70 | # ------------------------------------------------------------------------------------------------------------------ 71 | # TESTS 72 | # 73 | 74 | def test_batch_J_matches_full_only_pose(self): 75 | """Test that levenberg_marquardt_full() and levenberg_marquardt_only_pose() return the same result when 76 | differencing, virtual configs, and obstacle avoidance is disabled. 77 | """ 78 | set_seed() 79 | problem = self.fetch_arm__square 80 | plan = PlannerSearcher(problem.robot).generate_plan(problem, k=20, verbosity=0).plan 81 | n = 50 82 | x_seed = problem.robot.clamp_to_joint_limits((plan.q_path + 0.1 * torch.randn_like(plan.q_path))[0:n]) 83 | 84 | opt_params = get_pose_only_optimization_parameters(1.1, 1.0) 85 | opt_problem = OptimizationProblem(problem, x_seed, plan.target_path[0:n], 0, False, 1, None) 86 | opt_state = OptimizationState(x_seed.clone(), 0, time()) 87 | 88 | # 89 | x_batched, J_batched, r_batched = levenberg_marquardt_only_pose( 90 | opt_problem, opt_state, opt_params, return_residual=True 91 | ) 92 | x_full, J_full, r_full = levenberg_marquardt_full(opt_problem, opt_state, opt_params, return_residual=True) 93 | 94 | for i in range(n): 95 | torch.testing.assert_close( 96 | J_batched[i], J_full[i * 6 : (i + 1) * 6, i * 7 : (i + 1) * 7], atol=1e-5, rtol=0.0 97 | ) # 7 = ndof 98 | torch.testing.assert_close(r_batched[i], r_full[i * 6 : (i + 1) * 6], atol=1e-5, rtol=0.0) 99 | torch.testing.assert_close(x_batched, x_full, atol=0.005, rtol=0.0) 100 | print(make_text_green_or_red("test passed", True)) 101 | 102 | def test_cholesky(self): 103 | """Check that cholesky decomposition returns the same value as torch.linalg.solve""" 104 | 105 | opt_params = OptimizationParameters( 106 | # General 107 | seed_w_only_pose=False, 108 | # Alphas 109 | alpha_position=0.75, 110 | alpha_rotation=0.5, 111 | alpha_differencing=0.01, 112 | alpha_virtual_configs=None, 113 | alpha_self_collision=0.01, 114 | alpha_env_collision=0.025, 115 | # Pose 116 | pose_do_scale_down_satisfied=True, 117 | pose_ignore_satisfied_threshold_scale=0.5, 118 | pose_ignore_satisfied_scale_down=0.5, 119 | # Differencing 120 | use_differencing=True, 121 | differencing_do_ignore_satisfied=True, 122 | differencing_ignore_satisfied_margin_deg=1.0, 123 | differencing_ignore_satisfied_margin_cm=1.0, 124 | # Virtual Configs 125 | use_virtual_configs=False, 126 | virtual_configs=None, 127 | n_virtual_configs=None, 128 | # Collisions 129 | use_self_collisions=True, 130 | use_env_collisions=True, 131 | ) 132 | set_seed() 133 | problem = self.fetch_arm__square 134 | robot = problem.robot 135 | n = 50 136 | qs = to_torch(robot.sample_joint_angles(n)) 137 | target_path = robot.forward_kinematics(qs + 0.1 * torch.randn_like(qs)) 138 | 139 | opt_problem = OptimizationProblem(problem, qs.clone(), target_path, 0, False, 1, None) 140 | opt_state = OptimizationState(qs.clone(), 0, time()) 141 | lambd = 0.00001 142 | 143 | x_new_cholesky, J_chl, r_chl = levenberg_marquardt_full( 144 | opt_problem, opt_state, opt_params, lambd=lambd, return_residual=True 145 | ) 146 | x_new_naive, J_naive, r_naive = levenberg_marquardt_full_naive( 147 | opt_problem, opt_state, opt_params, lambd=lambd, return_residual=True 148 | ) 149 | 150 | torch.testing.assert_close(J_naive, J_chl, atol=5e-4, rtol=0.0) 151 | torch.testing.assert_close(r_naive, r_chl, atol=5e-4, rtol=0.0) 152 | torch.testing.assert_close(x_new_cholesky, x_new_naive, atol=5e-4, rtol=0.0) 153 | 154 | # TODO: update to use new LM code 155 | # def test_ignore_satisfied_mse(self): 156 | # """Test that pose error is ignored when below the success threshold""" 157 | 158 | # # Test 1: pose error is zero, mjac nonzero 159 | # qs = torch.tensor( 160 | # [ 161 | # [0.0] * 7, 162 | # [PI / 4] * 7, 163 | # [PI / 2] * 7, 164 | # [PI / 4] * 7, 165 | # [3 * PI / 4] * 7, 166 | # [PI] * 7, 167 | # ], 168 | # dtype=torch.float32, 169 | # device="cuda", 170 | # ) 171 | # poses = problem.robot.forward_kinematics(qs) 172 | # loss = optimizer.loss_fn(qs, poses, -1, 0)[0].item() 173 | # qdeltas = torch.rad2deg( 174 | # torch.tensor( 175 | # [ 176 | # [PI / 4] * 7, 177 | # [PI / 4] * 7, 178 | # [-PI / 4] * 7, 179 | # [PI / 2] * 7, 180 | # [PI / 4] * 7, 181 | # ], 182 | # dtype=torch.float32, 183 | # device="cuda", 184 | # ).abs() 185 | # ) 186 | # error = torch.maximum(qdeltas - constraints.max_allowed_mjac_deg, torch.zeros_like(qdeltas)) 187 | # expected = torch.pow(error, 2).mean().item() 188 | # self.assertAlmostEqual(loss, expected, msg=f"test 1 failed", delta=0.001) 189 | 190 | # # 191 | # # Test 2: mjac below threshold, pose error high 192 | # qs = torch.deg2rad( 193 | # torch.tensor( 194 | # [ 195 | # [0.0] * 7, 196 | # [1.0] * 7, 197 | # [constraints.max_allowed_mjac_deg - 0.1] * 7, 198 | # [1.0] * 7, 199 | # [0.0] * 7, 200 | # [-1.0] * 7, 201 | # ], 202 | # dtype=torch.float32, 203 | # device="cuda", 204 | # ) 205 | # ) 206 | # _, poses = problem.robot.sample_joint_angles_and_poses(6) 207 | # poses = to_torch(poses) 208 | # loss = optimizer.loss_fn(qs, poses, -1, 0)[0].item() 209 | 210 | # t_error_cm, R_error_deg = calculate_pose_error_cm_deg(problem.robot, qs, poses) 211 | # t_error_cm -= constraints.max_allowed_position_error_cm 212 | # R_error_deg -= constraints.max_allowed_rotation_error_deg 213 | # expected = torch.pow(t_error_cm, 2).mean().item() + torch.pow(R_error_deg, 2).mean().item() 214 | # self.assertAlmostEqual(loss, expected) 215 | 216 | 217 | if __name__ == "__main__": 218 | unittest.main() 219 | -------------------------------------------------------------------------------- /tests/planners_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import torch 4 | import numpy as np 5 | from jrl.robot import Robot 6 | from jrl.robots import Panda 7 | 8 | from cppflow.evaluation_utils import angular_changes, prismatic_changes 9 | from cppflow.utils import set_seed, to_torch 10 | from cppflow import config 11 | from cppflow.planners import Planner, PlannerSearcher, PlannerSettings, CppFlowPlanner 12 | from cppflow.data_type_utils import problem_from_filename 13 | from cppflow.data_types import Problem 14 | 15 | set_seed() 16 | 17 | DEVICE = config.DEVICE 18 | torch.set_printoptions(threshold=10000, precision=6, sci_mode=False, linewidth=200) 19 | 20 | 21 | def _get_initial_configuration(robot: Robot, target_pose_np: np.ndarray) -> torch.Tensor: 22 | for _ in range(25): 23 | initial_configuration = robot.inverse_kinematics_klampt(target_pose_np, positional_tolerance=5e-5)[0] 24 | if not robot.config_self_collides(initial_configuration): 25 | return to_torch(initial_configuration) 26 | raise RuntimeError("Could not find collision free initial configuration") 27 | 28 | 29 | def _value_in_tensor(t: torch.Tensor, v: float) -> bool: 30 | """Check if a value is in a tensor""" 31 | return ((t - v).abs() < 1e-8).any() 32 | 33 | 34 | torch.set_default_dtype(torch.float32) 35 | torch.set_default_device(config.DEVICE) 36 | 37 | 38 | class PlannerTest(unittest.TestCase): 39 | def setUp(self) -> None: 40 | self.planner_settings = PlannerSettings( 41 | k=175, 42 | tmax_sec=2.5, 43 | anytime_mode_enabled=False, 44 | do_rerun_if_large_dp_search_mjac=True, 45 | do_rerun_if_optimization_fails=True, 46 | do_return_search_path_mjac=True, 47 | ) 48 | self.panda = Panda() 49 | self.mocked_planner = Planner(self.planner_settings, self.panda, is_mock=True) 50 | 51 | # We assume the network width is 9 in this testsuite (see `IkflowModelParameters` in ikflow/model.py). Here is 52 | # where we verify that assumption 53 | self.assertEqual(self.mocked_planner._network_width, 9) # pylint: disable=protected-access 54 | 55 | def assert_tensor_is_unique(self, x: torch.Tensor): 56 | for i in range(x.shape[0]): 57 | for j in range(x.shape[1]): 58 | # there should be one match: itself 59 | matching = (x - x[i, j]).abs() < 1e-8 60 | matching_count = matching.sum().item() - 1 61 | if matching_count > 0: 62 | tupes = matching.nonzero(as_tuple=True) 63 | print(f"\nError: value at x[{i}, {j}]={x[i,j]} found at {matching_count} other locations:") 64 | # tupes[0]: row idxs 65 | # tupes[1]: column idxs 66 | for c, (ii, jj) in enumerate(zip(tupes[0], tupes[1])): 67 | print(f" {c}: x[{ii}, {jj}]=\t{x[ii,jj]}") 68 | self.assertEqual(matching_count, 0) 69 | 70 | def assert_nearly_uniform(self, x: torch.Tensor, lower_bound: float, upper_bound: float, max_delta: float = 0.03): 71 | self.assertGreater(x.min().item(), lower_bound) 72 | self.assertLess(x.max().item(), upper_bound) 73 | self.assertAlmostEqual(x.mean().item(), 0, delta=max_delta) 74 | 75 | def assert_latent_is_per_k(self, latent: torch.Tensor, target_path_n_waypoints: int, k: int): 76 | """Test that a batched latent tensor 'latent' has 'k' unique latent vectors, which are copied 77 | 'target_path_n_waypoints' times each. 78 | """ 79 | for i in range(k): 80 | offset = i * target_path_n_waypoints 81 | for j in range(target_path_n_waypoints): 82 | torch.testing.assert_close(latent[offset], latent[offset + j]) 83 | 84 | for ii in range(k): 85 | if i == ii: 86 | continue 87 | offset2 = ii * target_path_n_waypoints 88 | l1 = latent[offset : offset + target_path_n_waypoints] 89 | l2 = latent[offset2 : offset2 + target_path_n_waypoints] 90 | self.assertFalse((l1 == l2).any()) 91 | 92 | # ================================================================================================================== 93 | # Tests 94 | 95 | def test_Plan(self): 96 | problem = problem_from_filename("", filepath_override="tests/fetch__s__truncated.yaml") 97 | assert isinstance(problem.robot, Robot), f"problem.robot is {type(problem.robot)}, should be Robot" 98 | planner = Planner(problem.robot, is_mock=True) 99 | problem.target_path = problem.target_path[0:5, :] 100 | 101 | qpath = torch.randn((5, 8)) 102 | plan = planner._plan_from_qpath(qpath, problem) 103 | qpath_rot, qpath_pri = problem.robot.split_configs_to_revolute_and_prismatic(qpath) 104 | assert qpath_rot.shape == (5, 7) 105 | assert qpath_pri.shape == (5, 1), f"qpath_pri.shape should be (5, 1), is {qpath_pri.shape} (equals {qpath_pri})" 106 | 107 | # revolute 108 | self.assertAlmostEqual(plan.mjac_per_timestep_deg.max().item(), plan.mjac_deg) 109 | self.assertAlmostEqual(torch.rad2deg(angular_changes(qpath_rot).abs().max()), plan.mjac_deg) 110 | # prismatic 111 | self.assertAlmostEqual(plan.mjac_per_timestep_cm.max().item(), plan.mjac_cm) 112 | self.assertAlmostEqual(100 * prismatic_changes(qpath_pri).abs().max(), plan.mjac_cm) 113 | 114 | def test_batch_ik_order(self): 115 | """Check that the output is the same for the same input""" 116 | 117 | # Test #1: All zeros 118 | problem_name = "fetch_arm__s" 119 | problem = problem_from_filename(problem_name) 120 | planner = PlannerSearcher(problem.robot) 121 | verbosity = 0 122 | 123 | set_seed() # set seed to ensure that ikflow returns the same tensor back 124 | plan_pre_search, _, _ = planner.generate_plan( 125 | problem, k=15, verbosity=verbosity, batch_ik_order="pre_search", run_batch_ik=True 126 | ) 127 | set_seed() 128 | plan_post_search, _, _ = planner.generate_plan( 129 | problem, k=15, verbosity=verbosity, batch_ik_order="post_search", run_batch_ik=True 130 | ) 131 | set_seed() 132 | plan_no_batch_ik, _, _ = planner.generate_plan(problem, k=15, verbosity=verbosity, run_batch_ik=False) 133 | 134 | self.assertTrue(plan_pre_search.q_path.shape == plan_post_search.q_path.shape) 135 | self.assertGreater(np.absolute((plan_pre_search.q_path - plan_post_search.q_path)).max(), 1e-6) 136 | self.assertTrue(plan_pre_search.q_path.shape == plan_no_batch_ik.q_path.shape) 137 | self.assertGreater(np.absolute((plan_pre_search.q_path - plan_no_batch_ik.q_path)).max(), 1e-6) 138 | 139 | def test__get_k_ikflow_qpaths_same_input_same_output(self): 140 | """Check that the output is the same for the same input""" 141 | k = 2 142 | 143 | # Test #1: All zeros 144 | ee_path = torch.zeros((5, 7), device=DEVICE) 145 | batched_latents = torch.zeros((10, 9), device=DEVICE) 146 | ikflow_qpaths, _ = self.mocked_planner._get_k_ikflow_qpaths( # pylint: disable=protected-access 147 | k, ee_path, batched_latents, verbosity=0 148 | ) 149 | torch.testing.assert_close(ikflow_qpaths[0], ikflow_qpaths[1]) 150 | 151 | # Test #2: Changing end effector path 152 | ee_path = torch.zeros((5, 7), device=DEVICE) 153 | ee_path[:, 0] = torch.arange(5) 154 | 155 | batched_latents = torch.zeros((10, 9), device=DEVICE) 156 | ikflow_qpaths, _ = self.mocked_planner._get_k_ikflow_qpaths( # pylint: disable=protected-access 157 | k, ee_path, batched_latents, verbosity=0 158 | ) 159 | torch.testing.assert_close(ikflow_qpaths[0], ikflow_qpaths[1]) 160 | 161 | def test__get_k_ikflow_qpaths_no_repeats_fixed_ee_path(self): 162 | """Check that no elements from one qpath tensor are found in another. This should be the case when the two 163 | qpaths have different latent vectors 164 | """ 165 | k = 2 166 | ee_path = torch.zeros((5, 7), device=DEVICE) 167 | batched_latents = torch.zeros((10, 9), device=DEVICE) # latents for qpath1 are all 0 168 | batched_latents[5:, :] = 1.0 # latents for qpath2 are all 1 169 | 170 | # qpath1 and qpath2 should be completely different, because they have different latent vectors and the same 171 | # ee_path. Elements 172 | ikflow_qpaths, _ = self.mocked_planner._get_k_ikflow_qpaths( # pylint: disable=protected-access 173 | k, ee_path, batched_latents, verbosity=0, clamp_to_joint_limits=False 174 | ) 175 | qpath_0 = ikflow_qpaths[0] 176 | qpath_1 = ikflow_qpaths[1] 177 | self.assertEqual(qpath_0.shape, (5, 7)) 178 | self.assertEqual(qpath_1.shape, (5, 7)) 179 | 180 | # Check that no value from qpath[0] is in qpath[1]. This should happen with exceedingly low probability 181 | for i in range(5): 182 | for j in range(7): 183 | value_k0 = qpath_0[i, j] 184 | in_k2 = _value_in_tensor(qpath_1, value_k0).item() 185 | self.assertFalse( 186 | in_k2, 187 | f"Error: found value qpath_0[{i}, {j}]={value_k0} found in qpaths_1.\nqpaths_1={qpath_1.data}", 188 | ) 189 | 190 | def test__get_k_ikflow_qpaths_no_repeats_changing_ee_path(self): 191 | """Check that no elements match one another in returned qpath tensors. with a changing target pose value this 192 | should be the case 193 | 194 | Note: This test will fail if clamp_to_joint_limits is enabled. 195 | """ 196 | # Setup inputs 197 | k = 2 198 | ee_path = torch.zeros((5, 7), device=DEVICE) 199 | ee_path[:, 0] = torch.arange(5) 200 | batched_latents = torch.zeros((10, 9), device=DEVICE) # latents for qpath1 are all 0 201 | batched_latents[5:, :] = 1.0 # latents for qpath2 are all 1 202 | 203 | # Call ikflow 204 | ikflow_qpaths, _ = self.mocked_planner._get_k_ikflow_qpaths( # pylint: disable=protected-access 205 | k, 206 | ee_path, 207 | batched_latents, 208 | clamp_to_joint_limits=False, 209 | ) 210 | qpath_0 = ikflow_qpaths[0] 211 | qpath_1 = ikflow_qpaths[1] 212 | 213 | # Validate output 214 | self.assertEqual(qpath_0.shape, (5, 7)) 215 | self.assertEqual(qpath_1.shape, (5, 7)) 216 | self.assert_tensor_is_unique(qpath_0) 217 | self.assert_tensor_is_unique(qpath_1) 218 | 219 | def test__get_fixed_random_latent(self): 220 | """Sanity check _get_fixed_random_latent()""" 221 | # -------------------- 222 | # Test 1: Per-k 223 | k = 15 224 | target_path_n_waypoints = 300 225 | distribution = "uniform" 226 | latent_vector_scale = 1.5 227 | per_k_or_timestep = "per_k" 228 | latent = self.mocked_planner._get_fixed_random_latent( 229 | k, target_path_n_waypoints, distribution, latent_vector_scale, per_k_or_timestep 230 | ) 231 | # Test that the latents are nearly uniform, and also that there are k unique ones 232 | self.assertEqual(latent.shape, (4500, 9)) 233 | self.assert_nearly_uniform(latent, -0.75, 0.75) 234 | self.assert_latent_is_per_k(latent, target_path_n_waypoints, k) 235 | 236 | # gaussian 237 | distribution = "gaussian" 238 | latent = self.mocked_planner._get_fixed_random_latent( 239 | k, target_path_n_waypoints, distribution, latent_vector_scale, per_k_or_timestep 240 | ) 241 | self.assertEqual(latent.shape, (4500, 9)) 242 | self.assert_latent_is_per_k(latent, target_path_n_waypoints, k) 243 | 244 | # -------------------- 245 | # Test 2: Per timestep 246 | k = 5 247 | target_path_n_waypoints = 25 248 | distribution = "uniform" 249 | latent_vector_scale = 3.0 250 | per_k_or_timestep = "per_timestep" 251 | latent = self.mocked_planner._get_fixed_random_latent( 252 | k, target_path_n_waypoints, distribution, latent_vector_scale, per_k_or_timestep 253 | ) 254 | # Test that the latents are nearly uniform, and also that they are all unique 255 | self.assertEqual(latent.shape, (125, 9)) 256 | self.assert_nearly_uniform(latent, -1.5, 1.5) 257 | self.assert_tensor_is_unique(latent) 258 | 259 | distribution = "gaussian" 260 | latent = self.mocked_planner._get_fixed_random_latent( 261 | k, target_path_n_waypoints, distribution, latent_vector_scale, per_k_or_timestep 262 | ) 263 | self.assertEqual(latent.shape, (125, 9)) 264 | self.assert_tensor_is_unique(latent) 265 | 266 | # uv run python tests/planners_test.py PlannerTest.test_use_initial_configuration 267 | def test_use_initial_configuration(self): 268 | 269 | # Create a the planner 270 | settings = PlannerSettings( 271 | k=175, 272 | tmax_sec=3.0, 273 | anytime_mode_enabled=True, 274 | do_rerun_if_large_dp_search_mjac=True, 275 | do_rerun_if_optimization_fails=False, # TODO: test again with this True 276 | do_return_search_path_mjac=True, 277 | verbosity=2, 278 | ) 279 | planner = CppFlowPlanner(settings, self.panda) 280 | 281 | # Create a problem. This is the beginning of the panda__1cube problem 282 | xyz_offset = np.array([0, 0.5421984559194368, 0.7885155964931997]) 283 | target_path = np.array( 284 | [ 285 | [0.45, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], # x, y, z, qw, x, y, z 286 | [0.44547737, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 287 | [0.44095477, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 288 | [0.43643215, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 289 | [0.43190953, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 290 | [0.4273869, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 291 | [0.42286432, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 292 | [0.4183417, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 293 | [0.41381907, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 294 | [0.40929648, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 295 | [0.40477386, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 296 | ] 297 | ) 298 | target_path[:, 0:3] += xyz_offset 299 | # q0 = torch.tensor( 300 | # [1.267967, 0.711829, -0.811080, -0.810924, -2.637594, 1.767759, 0.083284], 301 | # device=DEVICE, 302 | # dtype=torch.float32, 303 | # ) 304 | q0 = _get_initial_configuration(self.panda, target_path[0]) 305 | 306 | target_path = to_torch(target_path) 307 | torch.testing.assert_close( 308 | target_path[0][None, :], self.panda.forward_kinematics(q0[None, :]), atol=0.001, rtol=0.0 309 | ) 310 | 311 | problem = Problem( 312 | target_path=target_path, 313 | initial_configuration=q0[None, :], 314 | robot=self.panda, 315 | name="test-problem", 316 | full_name="test-problem", 317 | obstacles=[], 318 | obstacles_Tcuboids=[], 319 | obstacles_cuboids=[], 320 | obstacles_klampt=[], 321 | ) 322 | 323 | # Solve 324 | plan = planner.generate_plan(problem).plan 325 | print(plan) 326 | 327 | # Check that the first q in the plan is the initial configuration 328 | print("plan.q_path[0]:", plan.q_path[0]) 329 | print("q0: ", q0) 330 | print("difference: ", plan.q_path[0] - q0) 331 | print("norm distance: ", torch.norm(plan.q_path[0] - q0).item()) 332 | torch.testing.assert_close(plan.q_path[0], q0, atol=1e-5, rtol=0.0) 333 | 334 | 335 | if __name__ == "__main__": 336 | unittest.main() 337 | -------------------------------------------------------------------------------- /tests/problem_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from cppflow.problem import problem_from_filename, Problem 4 | 5 | import numpy as np 6 | 7 | 8 | class ProblemTest(unittest.TestCase): 9 | def _get_problem(self, target_path): 10 | return Problem( 11 | target_path=target_path, 12 | robot=None, 13 | name=None, 14 | full_name=None, 15 | obstacles=[], 16 | ) 17 | 18 | # Tests 19 | # TODO 20 | def test_offset_target_path(self): 21 | pass 22 | 23 | def test_problem(self): 24 | problem = problem_from_filename("panda__square") 25 | self.assertIsInstance(problem, Problem) 26 | 27 | def test_path_length_calculation(self): 28 | target_path = np.array( 29 | [ 30 | [0, 0, 0, 1.0, 0, 0, 0], 31 | [0, 0, 0, 1.0, 0, 0, 0], 32 | [0, 0, 0, 1.0, 0, 0, 0], 33 | [0, 0, 0, 1.0, 0, 0, 0], 34 | ] 35 | ) 36 | problem = self._get_problem(target_path) 37 | 38 | # Position 39 | expected_pos = 0.0 40 | expected_rot = 0.0 41 | self.assertAlmostEqual(expected_pos, problem.path_length_cumultive_positional_change_cm) 42 | self.assertAlmostEqual(expected_rot, problem.path_length_cumulative_rotational_change_deg) 43 | 44 | # ------ 45 | # Test 2 46 | # quaternions from https://www.andre-gaschler.com/rotationconverter/ 47 | target_path = np.array( 48 | [ 49 | [0, 0, 0, 1.0, 0, 0, 0], 50 | [0.1, 0, 0, 0.9990482, 0, 0, 0.0436194], # [ 0.9990482, 0, 0, 0.0436194 ] - 5 deg about [0, 0, 1] 51 | [0.1, 0, 0, 0.9914449, 0, 0, 0.1305262], # [ 0.9914449, 0, 0, 0.1305262 ] - 15 deg about [0, 0, 1] 52 | [0.2, 0, 0, 0.9063078, 0, 0, 0.4226183], # [ 0.9063078, 0, 0, 0.4226183 ] - 50 deg about [0, 0, 1] 53 | ] 54 | ) 55 | problem = self._get_problem(target_path) 56 | # 57 | expected_pos = 20.0 58 | expected_rot = 5.0 + 10.0 + 35.0 59 | self.assertAlmostEqual(expected_pos, problem.path_length_cumultive_positional_change_cm) 60 | self.assertAlmostEqual(expected_rot, problem.path_length_cumulative_rotational_change_deg, places=4) 61 | 62 | # ------ 63 | # Test 3 64 | # triangle values from https://www.calculator.net/right-triangle-calculator.html? 65 | target_path = np.array( 66 | [ 67 | [0, 0, 0, 1.0, 0, 0, 0], 68 | [1, 2, 0, 0.9990482, 0.0, 0.0, 0.0436194], # [0, 0, 5] 69 | [0, 0, 0, 0.9952465, 0.0870728, -0.0038017, 0.0434534], # [10, 0, 5] 70 | [0, 2, 7, 0.9941335, 0.0888853, 0.039614, 0.0472101], # [10, 5, 5 ] 71 | ] 72 | ) 73 | problem = self._get_problem(target_path) 74 | # 75 | expected_pos = (2.23607 + 2.23607 + 7.28011) * 100 76 | expected_rot = 5.0 + 10.0 + 5.0 77 | self.assertAlmostEqual(expected_pos, problem.path_length_cumultive_positional_change_cm, places=3) 78 | self.assertAlmostEqual(expected_rot, problem.path_length_cumulative_rotational_change_deg, places=3) 79 | 80 | 81 | if __name__ == "__main__": 82 | unittest.main() 83 | -------------------------------------------------------------------------------- /tests/search_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import torch 4 | import numpy as np 5 | from jrl.robots import Fetch 6 | 7 | from cppflow.search import joint_limit_almost_violations_3d 8 | from cppflow.problem import problem_from_filename 9 | from cppflow.data_types import PlannerSearcher 10 | from cppflow.utils import set_seed 11 | 12 | set_seed() 13 | 14 | torch.set_default_dtype(torch.float32) 15 | torch.set_default_device("cuda:0") 16 | torch.set_printoptions(linewidth=200) 17 | np.set_printoptions(linewidth=500) 18 | 19 | 20 | class SearchTest(unittest.TestCase): 21 | # uv run python tests/search_test.py SearchTest.test_joint_limit_almost_violations 22 | def test_joint_limit_almost_violations(self): 23 | robot = Fetch() 24 | # qs = torch.zeros((1, 3, 8)) # [k x ntimesteps x n_dofs] 25 | qs = torch.zeros((2, 3, 8)) # [k x ntimesteps x n_dofs] 26 | qs[0, 0] = torch.tensor([0.051, 0, 0, 0, 0, 0, 0, 0]) # all good 27 | qs[0, 1] = torch.tensor([0.38615 - 0.001, 0, 0, 0, 0, 0, 0, 0]) # prismatic is out of bounds 28 | qs[0, 2] = torch.tensor([0.38615 - 0.051, 0, 0, 0, 0, 0, 0, 0]) # all good 29 | qs[1, 0] = torch.tensor([0.38615 - 0.051, 0, 0, -np.pi, 0, 0, 0, 0]) # out of bounds 30 | qs[1, 1] = torch.tensor([0.38615 - 0.051, 0, 0, -np.pi + 0.11, 0, 0, 0, 0]) # all good 31 | qs[1, 2] = torch.tensor([0.38615 - 0.051, 0, 0, -np.pi + 0.11, 0, 0, 0, np.pi - 0.25]) # all good 32 | eps_revolute = 0.1 33 | eps_prismatic = 0.05 34 | 35 | # (0, 0.38615), 36 | # (-1.6056, 1.6056), 37 | # (-1.221, 1.518), # shoulder_lift_joint 38 | # (-np.pi, np.pi), # upperarm_roll_joint 39 | # (-2.251, 2.251), # elbow_flex_joint 40 | # (-np.pi, np.pi), # forearm_roll_joint 41 | # (-2.16, 2.16), # wrist_flex_joint 42 | # (-np.pi, np.pi), # wrist_roll_joint 43 | # expected = torch.zeros((1, 3)) # [k x ntimesteps] 44 | expected = torch.zeros((2, 3)) # [k x ntimesteps] 45 | expected[0, 0] = 0 46 | expected[0, 1] = 1 47 | expected[0, 2] = 0 48 | 49 | expected[1, 0] = 1 50 | expected[1, 1] = 0 51 | expected[1, 2] = 0 52 | 53 | returned = joint_limit_almost_violations_3d(robot, qs, eps_revolute=eps_revolute, eps_prismatic=eps_prismatic) 54 | print("\nexpected:\n", expected) 55 | print("returned:\n", returned) 56 | print("\ndiff:\n", expected - returned) 57 | torch.testing.assert_close(returned, expected) 58 | 59 | def test_joint_limit_avoidance(self): 60 | """Test that the optimal path is returned by dp_search""" 61 | problem_name = "tests/fetch__s__truncated.yaml" 62 | problem = problem_from_filename("", filepath_override=problem_name) 63 | planner = PlannerSearcher(problem.robot) 64 | k = 20 65 | result = planner.generate_plan(problem, k=k) 66 | plan = result.plan 67 | other_plans = result.other_plans 68 | eps = np.deg2rad(1) 69 | for joint_idx, (l, u) in enumerate(problem.robot.actuated_joints_limits): 70 | violating_l = plan.q_path[:, joint_idx] < l + eps 71 | violating_u = plan.q_path[:, joint_idx] > u - eps 72 | self.assertFalse(torch.any(violating_l)) 73 | self.assertFalse(torch.any(violating_u)) 74 | for op in other_plans: 75 | self.assertLessEqual(plan.mjac, op.mjac, msg=f" BUG FOUND - {plan.mjac} > {op.mjac}") 76 | 77 | 78 | if __name__ == "__main__": 79 | unittest.main() 80 | -------------------------------------------------------------------------------- /tests/test_data/test_problem.yaml: -------------------------------------------------------------------------------- 1 | robot: "panda" 2 | path_name: "square" 3 | 4 | path_xyz_offset: [0.75, 0, 0.5] 5 | path_R_offset: [ 6 | [1, 0, 0], 7 | [0, 1, 0], 8 | [0, 0, 1] 9 | ] # rotation about +y by 90deg. This makes the gripper pointing in the same direction as the fetch robot is in the TORM demo -------------------------------------------------------------------------------- /tests/utils_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from cppflow.utils import calc_hash, TestSpecification 4 | 5 | 6 | class UtilsTest(unittest.TestCase): 7 | def test_calc_hash(self): 8 | in_1 = {"verbosity": 0, "k": 25} 9 | in_2 = {"verbosity": 0, "k": 50} 10 | print(calc_hash(in_1), calc_hash(in_2)) 11 | self.assertNotEqual(calc_hash(in_1), calc_hash(in_2)) 12 | 13 | in_1 = {"verbosity": 0, "k": 35} 14 | in_2 = {"verbosity": 0, "k": 35} 15 | print(calc_hash(in_1), calc_hash(in_2)) 16 | self.assertEqual(calc_hash(in_1), calc_hash(in_2)) 17 | 18 | in_1 = {"verbosity": 0, "k": 45} 19 | in_2 = {"k": 45, "verbosity": 0} 20 | print(calc_hash(in_1), calc_hash(in_2)) 21 | self.assertEqual(calc_hash(in_1), calc_hash(in_2)) 22 | 23 | def test_TestSpecification(self): 24 | t1 = TestSpecification(planner="CppFlowPlanner", problem="panda__1cube", k=55, n_runs=3) 25 | t2 = TestSpecification(planner="CppFlowPlanner", problem="panda__1cube", k=55, n_runs=3) 26 | print(t1.get_hash(), t2.get_hash()) 27 | self.assertEqual(t1.get_hash(), t2.get_hash()) 28 | 29 | 30 | if __name__ == "__main__": 31 | unittest.main() 32 | --------------------------------------------------------------------------------