scikit-robot 0.4.4__tar.gz → 0.4.6__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {scikit_robot-0.4.4/scikit_robot.egg-info → scikit_robot-0.4.6}/PKG-INFO +1 -1
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/pyproject.toml +1 -1
- {scikit_robot-0.4.4 → scikit_robot-0.4.6/scikit_robot.egg-info}/PKG-INFO +1 -1
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/collision/__init__.py +2 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/collision/robot_collision.py +63 -4
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/collision/self_collision.py +53 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/coordinates/base.py +7 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros2/base.py +20 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/joint.py +127 -11
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/robot_model.py +148 -12
- scikit_robot-0.4.6/skrobot/planner/swept_sphere.py +277 -0
- scikit_robot-0.4.6/skrobot/pycompat.py +63 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/sdf/signed_distance_function.py +93 -0
- scikit_robot-0.4.4/skrobot/planner/swept_sphere.py +0 -136
- scikit_robot-0.4.4/skrobot/pycompat.py +0 -41
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/LICENSE +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/MANIFEST.in +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/README.md +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/scikit_robot.egg-info/SOURCES.txt +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/scikit_robot.egg-info/dependency_links.txt +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/scikit_robot.egg-info/entry_points.txt +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/scikit_robot.egg-info/not-zip-safe +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/scikit_robot.egg-info/requires.txt +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/scikit_robot.egg-info/top_level.txt +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/setup.cfg +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/_lazy_imports.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/canonicalize_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/change_urdf_root.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/cli.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/convert_urdf_mesh.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/convert_urdf_to_mjcf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/convert_urdf_to_primitives.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/convert_urdf_to_usd.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/convert_wheel_collision.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/extract_sub_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/generate_robot_class.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/modularize_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/scale_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/transform_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/urdf_hash.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/urdf_tree.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/visualize_mesh.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/apps/visualize_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/assembly/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/assembly/module_assembly.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/assembly/module_registry.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/backend/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/backend/jax_backend.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/backend/math_utils.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/backend/numpy_backend.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/backend/registry.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/collision/distance.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/collision/environment_collision.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/collision/geometry.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/coordinates/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/coordinates/geo.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/coordinates/math.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/kuka.urdf +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/finger_base_left.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/finger_base_right.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/finger_tip_left.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/finger_tip_right.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_0.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_0.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_0.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_1.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_1.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_1.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_2.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_2.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_2.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_3.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_3.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_3.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_4.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_4.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_4.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_5.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_5.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_5.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_6.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_6.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_6.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_7.mtl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_7.obj +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/kuka_description/meshes/link_7.stl +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/robot_descriptions/urdf/r8_6.urdf +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/CMakeLists.txt.ros2.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/gazebo.launch.py.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/gazebo_moveit.launch.py.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/isaac_fjt_server.py.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/moveit.rviz.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/package.xml.ros2.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/ros2_control.xacro.joint.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/ros2_control.xacro.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/data/ros_config_templates/setup_assistant.template +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/dynamics/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/dynamics/differentiable.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/dynamics/gravity.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/export/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/export/contact_links.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/export/isaac/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/export/isaac/fjt_server.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/export/task.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/export/usd.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/_pybullet.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/base.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/move_base.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/nextage.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/panda.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/pr2.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/tf_utils.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros/transform_listener.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros2/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros2/gripper.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/interfaces/ros2/panda.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/_base_placement_jax.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/base_placement.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/differentiable.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/inverse_reachability_map.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/loop_closure.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/kinematics/reachability_map.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/joint_limit_table.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/link.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/primitives.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/model/skeleton.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/aero.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/differential_wrist_sample.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/fetch.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/g1.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/griphis.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/hydrus.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/jaxon_jvrc.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/jedy.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/kuka.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/nextage.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/panda.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/pr2.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/r8_6.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/tycoon.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/models/urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/collision_checker.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/inverse_kinematics_optimization.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/sqp_based.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/collision.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/fk_utils.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/gridsdf_collision.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/problem.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/residuals.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/augmented_lagrangian.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/base.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/gradient_descent.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/jaxls_solver.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/scipy_solver.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/solvers/solver_utils.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/trajectory.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/trajectory_optimization/world_surface_collision.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/planner/utils.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/sdf/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/aggregate.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/canonicalize.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/compare.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/extract_sub_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/flip_axis.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/hash.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/link_origins.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/llm_grouping.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/merge_fixed_links.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/modularize_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/primitives_converter.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/robot_class_generator.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_config/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_config/export.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_config/gazebo_generator.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_config/moveit_generator.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_config/moveit_package.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_config/urdf_parser.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_package.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/ros_package_exporter.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/sanitize.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/scale_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/structure.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/transform_urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/wheel_collision_converter.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/urdf/xml_root_link_changer.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/_blender_decimate_core.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/_blender_remesh_core.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/archive.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/blender_mesh.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/checksum.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/convex_decomposition.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/draco.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/inertia.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/listify.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/mesh.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/mjcf_converter.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/package.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/primitive_fitting.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/urdf.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/urdf_mesh.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/video.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/utils/visualization.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/__init__.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_base.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_manipulability_ellipse.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_mitsuba.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_notebook.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_offscreen.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_path_utils.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_pyrender.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_trimesh.py +0 -0
- {scikit_robot-0.4.4 → scikit_robot-0.4.6}/skrobot/viewers/_viser.py +0 -0
|
@@ -64,6 +64,7 @@ from skrobot.collision.self_collision import link_visual_mesh
|
|
|
64
64
|
from skrobot.collision.self_collision import moving_links
|
|
65
65
|
from skrobot.collision.self_collision import SelfCollision
|
|
66
66
|
from skrobot.collision.self_collision import sweep_limits
|
|
67
|
+
from skrobot.collision.self_collision import touching_link_pairs
|
|
67
68
|
|
|
68
69
|
|
|
69
70
|
__all__ = [
|
|
@@ -99,4 +100,5 @@ __all__ = [
|
|
|
99
100
|
'moving_links',
|
|
100
101
|
'link_visual_mesh',
|
|
101
102
|
'is_fcl_available',
|
|
103
|
+
'touching_link_pairs',
|
|
102
104
|
]
|
|
@@ -103,7 +103,8 @@ class RobotCollisionChecker:
|
|
|
103
103
|
self.color_collision_sphere = [255, 0, 0, 200]
|
|
104
104
|
|
|
105
105
|
def add_link(self, link, geometry_type='auto', n_spheres=None,
|
|
106
|
-
radius_scale=1.0, tol=0.1, aspect_threshold=1.5
|
|
106
|
+
radius_scale=1.0, tol=0.1, aspect_threshold=1.5,
|
|
107
|
+
fit_tol=0.01):
|
|
107
108
|
"""Add collision geometry for a link.
|
|
108
109
|
|
|
109
110
|
Automatically generates collision geometry from the link's
|
|
@@ -119,6 +120,9 @@ class RobotCollisionChecker:
|
|
|
119
120
|
- 'auto': Automatically select based on aspect ratio (default)
|
|
120
121
|
- 'spheres': Use swept spheres (compatible with SweptSphereSdfCollisionChecker)
|
|
121
122
|
- 'capsule': Use single capsule (more efficient for elongated shapes)
|
|
123
|
+
- 'fitted_spheres': Spheres of varying radius covering the mesh's
|
|
124
|
+
convex hull, none reaching more than about ``fit_tol`` out of it
|
|
125
|
+
(see :func:`skrobot.planner.swept_sphere.compute_fitted_spheres`)
|
|
122
126
|
n_spheres : int or None
|
|
123
127
|
Number of spheres to use (only for 'spheres' type). If None,
|
|
124
128
|
automatically determined based on tolerance.
|
|
@@ -129,6 +133,9 @@ class RobotCollisionChecker:
|
|
|
129
133
|
aspect_threshold : float
|
|
130
134
|
Aspect ratio threshold for auto geometry selection.
|
|
131
135
|
If length/diameter > threshold, use capsule. Default is 1.5.
|
|
136
|
+
fit_tol : float
|
|
137
|
+
How far a sphere may reach out of the mesh's convex hull, in
|
|
138
|
+
metres (only for 'fitted_spheres' type).
|
|
132
139
|
"""
|
|
133
140
|
try:
|
|
134
141
|
import trimesh
|
|
@@ -141,6 +148,14 @@ class RobotCollisionChecker:
|
|
|
141
148
|
self.add_link_sphere(link, radius=0.05)
|
|
142
149
|
return
|
|
143
150
|
|
|
151
|
+
if geometry_type == 'fitted_spheres':
|
|
152
|
+
from skrobot.planner.swept_sphere import compute_fitted_spheres
|
|
153
|
+
centers, radii = compute_fitted_spheres(mesh, tol=fit_tol)
|
|
154
|
+
for center, radius in zip(centers, radii):
|
|
155
|
+
self.add_link_sphere(link, center_local=center,
|
|
156
|
+
radius=radius * radius_scale)
|
|
157
|
+
return
|
|
158
|
+
|
|
144
159
|
try:
|
|
145
160
|
# Compute capsule parameters from mesh
|
|
146
161
|
p1, p2, radius = self._compute_capsule_from_mesh(mesh)
|
|
@@ -238,7 +253,7 @@ class RobotCollisionChecker:
|
|
|
238
253
|
return p1, p2, radius
|
|
239
254
|
|
|
240
255
|
def add_links(self, links, geometry_type='auto', n_spheres=None,
|
|
241
|
-
radius_scale=1.0, aspect_threshold=1.5):
|
|
256
|
+
radius_scale=1.0, aspect_threshold=1.5, fit_tol=0.01):
|
|
242
257
|
"""Add collision geometry for multiple links.
|
|
243
258
|
|
|
244
259
|
Parameters
|
|
@@ -246,13 +261,17 @@ class RobotCollisionChecker:
|
|
|
246
261
|
links : list of Link
|
|
247
262
|
Robot links to add.
|
|
248
263
|
geometry_type : str
|
|
249
|
-
Type of geometry: 'auto', 'spheres',
|
|
264
|
+
Type of geometry: 'auto', 'spheres', 'capsule' or
|
|
265
|
+
'fitted_spheres'.
|
|
250
266
|
n_spheres : int or None
|
|
251
267
|
Number of spheres per link (only for 'spheres' type).
|
|
252
268
|
radius_scale : float
|
|
253
269
|
Scale factor for computed radius.
|
|
254
270
|
aspect_threshold : float
|
|
255
271
|
Aspect ratio threshold for auto selection.
|
|
272
|
+
fit_tol : float
|
|
273
|
+
How far a sphere may reach out of a link's convex hull, in
|
|
274
|
+
metres (only for 'fitted_spheres' type).
|
|
256
275
|
"""
|
|
257
276
|
for link in links:
|
|
258
277
|
self.add_link(
|
|
@@ -261,6 +280,7 @@ class RobotCollisionChecker:
|
|
|
261
280
|
n_spheres=n_spheres,
|
|
262
281
|
radius_scale=radius_scale,
|
|
263
282
|
aspect_threshold=aspect_threshold,
|
|
283
|
+
fit_tol=fit_tol,
|
|
264
284
|
)
|
|
265
285
|
|
|
266
286
|
def add_link_sphere(self, link, center_local=None, radius=0.05):
|
|
@@ -402,7 +422,9 @@ class RobotCollisionChecker:
|
|
|
402
422
|
|
|
403
423
|
def setup_self_collision_pairs(self, min_link_distance=2,
|
|
404
424
|
ignore_pairs=None,
|
|
405
|
-
use_urdf_adjacency=True
|
|
425
|
+
use_urdf_adjacency=True,
|
|
426
|
+
ignore_touching_at=None,
|
|
427
|
+
touching_margin=0.0):
|
|
406
428
|
"""Setup pairs of link geometries for self-collision checking.
|
|
407
429
|
|
|
408
430
|
Parameters
|
|
@@ -416,6 +438,18 @@ class RobotCollisionChecker:
|
|
|
416
438
|
use_urdf_adjacency : bool
|
|
417
439
|
If True, compute actual kinematic chain distance using
|
|
418
440
|
parent-child relationships. If False, use insertion order.
|
|
441
|
+
ignore_touching_at : list of array-like, optional
|
|
442
|
+
Angle vectors (the robot's rest or tucked pose, say). A link
|
|
443
|
+
pair whose geometries are closer than ``touching_margin`` in
|
|
444
|
+
any of them is not checked. The spheres and capsules are
|
|
445
|
+
fatter than the links, so pairs such as a folded arm resting
|
|
446
|
+
over its base can overlap in the robot's own rest pose; kept,
|
|
447
|
+
they make every self-collision constraint unsatisfiable from
|
|
448
|
+
that pose. The robot's angle vector is restored afterwards.
|
|
449
|
+
touching_margin : float
|
|
450
|
+
Distance below which a pair counts as touching in
|
|
451
|
+
``ignore_touching_at`` (the clearance an optimizer is asked
|
|
452
|
+
for, typically).
|
|
419
453
|
"""
|
|
420
454
|
self._self_collision_pairs = []
|
|
421
455
|
n = len(self._link_geometries)
|
|
@@ -435,6 +469,10 @@ class RobotCollisionChecker:
|
|
|
435
469
|
ignore_set.add((name_a, name_b))
|
|
436
470
|
ignore_set.add((name_b, name_a))
|
|
437
471
|
|
|
472
|
+
if ignore_touching_at is not None:
|
|
473
|
+
ignore_set |= self._touching_link_pairs(ignore_touching_at,
|
|
474
|
+
touching_margin)
|
|
475
|
+
|
|
438
476
|
# Compute kinematic chain distance between links
|
|
439
477
|
def get_ancestors(link):
|
|
440
478
|
"""Get list of ancestor link names from link to root."""
|
|
@@ -501,6 +539,27 @@ class RobotCollisionChecker:
|
|
|
501
539
|
|
|
502
540
|
self._self_collision_pairs.append((i, j))
|
|
503
541
|
|
|
542
|
+
def _touching_link_pairs(self, angle_vectors, margin):
|
|
543
|
+
"""Link-name pairs (both orders) whose geometries come closer than
|
|
544
|
+
margin in any of angle_vectors."""
|
|
545
|
+
saved = self.robot_model.angle_vector().copy()
|
|
546
|
+
out = set()
|
|
547
|
+
try:
|
|
548
|
+
for av in angle_vectors:
|
|
549
|
+
self.robot_model.angle_vector(np.asarray(av, dtype=float))
|
|
550
|
+
world = [lg.get_world_geometry() for lg in self._link_geometries]
|
|
551
|
+
names = [lg.link.name for lg in self._link_geometries]
|
|
552
|
+
for i in range(len(world)):
|
|
553
|
+
for j in range(i + 1, len(world)):
|
|
554
|
+
if names[i] == names[j] or (names[i], names[j]) in out:
|
|
555
|
+
continue
|
|
556
|
+
if collision_distance(world[i], world[j]) < margin:
|
|
557
|
+
out.add((names[i], names[j]))
|
|
558
|
+
out.add((names[j], names[i]))
|
|
559
|
+
finally:
|
|
560
|
+
self.robot_model.angle_vector(saved)
|
|
561
|
+
return out
|
|
562
|
+
|
|
504
563
|
def set_self_collision_pairs(self, pairs):
|
|
505
564
|
"""Manually set self-collision pairs.
|
|
506
565
|
|
|
@@ -273,6 +273,59 @@ class SelfCollision:
|
|
|
273
273
|
return float(d), tuple(sorted((la, lb)))
|
|
274
274
|
|
|
275
275
|
|
|
276
|
+
def touching_link_pairs(robot, angle_vectors=None, meshes=None, hull=False):
|
|
277
|
+
"""Link pairs whose meshes touch in the given poses, and every
|
|
278
|
+
parent/child pair.
|
|
279
|
+
|
|
280
|
+
What MoveIt's setup assistant disables as ``Default`` and ``Adjacent``:
|
|
281
|
+
pairs in contact in the robot's own rest pose (parts touching by design,
|
|
282
|
+
a folded arm lying on its base) can never be told apart from a collision
|
|
283
|
+
by a self-collision check there, so they go in ``ignore_pairs`` of
|
|
284
|
+
:meth:`RobotCollisionChecker.setup_self_collision_pairs` or the SRDF's
|
|
285
|
+
disabled collisions. Needs the optional ``python-fcl`` package.
|
|
286
|
+
|
|
287
|
+
Parameters
|
|
288
|
+
----------
|
|
289
|
+
robot : skrobot.model.RobotModel
|
|
290
|
+
angle_vectors : list of array-like, optional
|
|
291
|
+
Poses to look in; the robot's current pose when omitted. The
|
|
292
|
+
robot's angle vector is restored afterwards.
|
|
293
|
+
meshes : dict, optional
|
|
294
|
+
``{link name: local-frame trimesh}``; :func:`link_meshes` by default.
|
|
295
|
+
hull : bool
|
|
296
|
+
Use each link's convex hull (faster, and more pairs touch).
|
|
297
|
+
|
|
298
|
+
Returns
|
|
299
|
+
-------
|
|
300
|
+
pairs : set of tuple of str
|
|
301
|
+
``(link_a, link_b)`` with ``link_a < link_b``.
|
|
302
|
+
"""
|
|
303
|
+
if meshes is None:
|
|
304
|
+
meshes = link_meshes(robot)
|
|
305
|
+
saved = robot.angle_vector().copy()
|
|
306
|
+
pairs = set()
|
|
307
|
+
sc = None
|
|
308
|
+
try:
|
|
309
|
+
for av in ([None] if angle_vectors is None else angle_vectors):
|
|
310
|
+
if av is not None:
|
|
311
|
+
robot.angle_vector(np.asarray(av, dtype=float))
|
|
312
|
+
if sc is None:
|
|
313
|
+
# its baseline: contacts here, and parent/child pairs
|
|
314
|
+
sc = SelfCollision(robot, meshes, hull=hull)
|
|
315
|
+
pairs |= sc.baseline
|
|
316
|
+
else:
|
|
317
|
+
sc._sync()
|
|
318
|
+
pairs |= sc._pairs(0.0)
|
|
319
|
+
finally:
|
|
320
|
+
robot.angle_vector(saved)
|
|
321
|
+
# parent/child from the link tree: joint_list leaves fixed joints out
|
|
322
|
+
for link in robot.link_list:
|
|
323
|
+
parent = getattr(link, 'parent_link', None)
|
|
324
|
+
if parent is not None:
|
|
325
|
+
pairs.add(frozenset((link.name, parent.name)))
|
|
326
|
+
return {tuple(sorted(p)) for p in pairs}
|
|
327
|
+
|
|
328
|
+
|
|
276
329
|
def _prismatic_joints(robot):
|
|
277
330
|
from skrobot.model.joint import LinearJoint
|
|
278
331
|
return [j for j in robot.joint_list if isinstance(j, LinearJoint)]
|
|
@@ -2099,6 +2099,13 @@ class CascadedCoords(Coordinates):
|
|
|
2099
2099
|
def transform(self, c, wrt='local', out=None):
|
|
2100
2100
|
"""Transform this coordinates
|
|
2101
2101
|
|
|
2102
|
+
Note that this function changes this coordinates
|
|
2103
|
+
translation and rotation, as Coordinates.transform does.
|
|
2104
|
+
For a link or any CascadedCoords with descendants, that also
|
|
2105
|
+
moves everything attached to it.
|
|
2106
|
+
If you would like not to change this coordinates,
|
|
2107
|
+
Please use copy_worldcoords() or give `out`.
|
|
2108
|
+
|
|
2102
2109
|
Parameters
|
|
2103
2110
|
----------
|
|
2104
2111
|
c : skrobot.coordinates.Coordinates
|
|
@@ -330,6 +330,10 @@ class ROS2RobotInterfaceBase(Node):
|
|
|
330
330
|
self._timeout_reason = ""
|
|
331
331
|
return True
|
|
332
332
|
|
|
333
|
+
if not self.context.ok():
|
|
334
|
+
self._timeout_reason = (
|
|
335
|
+
"ROS context shut down before joint_states were updated")
|
|
336
|
+
return False
|
|
333
337
|
if timeout and (time.time() - start) > timeout:
|
|
334
338
|
self._set_timeout_reason(initial_time)
|
|
335
339
|
logger.warning(
|
|
@@ -939,6 +943,15 @@ class ROS2RobotInterfaceBase(Node):
|
|
|
939
943
|
# Wait for goal acceptance
|
|
940
944
|
while not action._current_future.done():
|
|
941
945
|
time.sleep(0.01)
|
|
946
|
+
# After Ctrl-C the executor stops, so the future never
|
|
947
|
+
# completes; stop waiting instead of hanging the thread.
|
|
948
|
+
if not self.context.ok():
|
|
949
|
+
logger.warning(
|
|
950
|
+
"ROS context shut down while waiting for goal "
|
|
951
|
+
"acceptance")
|
|
952
|
+
results.append(True)
|
|
953
|
+
action_completed = True
|
|
954
|
+
break
|
|
942
955
|
if timeout > 0 and (time.time() - start_time) > timeout:
|
|
943
956
|
self.get_logger().warn(f"Goal acceptance timed out after {timeout}s")
|
|
944
957
|
results.append(True)
|
|
@@ -973,6 +986,13 @@ class ROS2RobotInterfaceBase(Node):
|
|
|
973
986
|
while not result_future.done():
|
|
974
987
|
time.sleep(0.01) # 10ms polling
|
|
975
988
|
|
|
989
|
+
if not self.context.ok():
|
|
990
|
+
logger.warning(
|
|
991
|
+
"ROS context shut down while waiting for motion")
|
|
992
|
+
results.append(True) # Still interpolating
|
|
993
|
+
action_completed = True
|
|
994
|
+
break
|
|
995
|
+
|
|
976
996
|
# Check timeout
|
|
977
997
|
if timeout > 0 and (time.time() - start_time) > timeout:
|
|
978
998
|
self.get_logger().warn(f"wait_interpolation timed out after {timeout}s")
|
|
@@ -3,9 +3,11 @@ import math
|
|
|
3
3
|
|
|
4
4
|
import numpy as np
|
|
5
5
|
|
|
6
|
+
from skrobot.coordinates import CascadedCoords
|
|
6
7
|
from skrobot.coordinates import convert_to_axis_vector
|
|
7
8
|
from skrobot.coordinates import normalize_vector
|
|
8
9
|
from skrobot.coordinates.math import cross_product
|
|
10
|
+
from skrobot.coordinates.math import rotation_matrix
|
|
9
11
|
from skrobot.coordinates.math import select_by_mask
|
|
10
12
|
|
|
11
13
|
|
|
@@ -13,6 +15,10 @@ logger = getLogger(__name__)
|
|
|
13
15
|
|
|
14
16
|
_default_max_joint_velocity = 1.0
|
|
15
17
|
_default_max_joint_torque = 1.0
|
|
18
|
+
# How far past a limit a joint angle [rad or m] is taken as rounding (a float32
|
|
19
|
+
# solution comes back as 1.5700000524 for a limit of 1.57): clamped without a
|
|
20
|
+
# warning. Anything further is still clamped, and warned about.
|
|
21
|
+
_limit_rounding = 1e-6
|
|
16
22
|
|
|
17
23
|
|
|
18
24
|
def calc_angle_speed_gain_scalar(joint, dav, i, periodic_time):
|
|
@@ -129,6 +135,106 @@ class Joint(object):
|
|
|
129
135
|
self.joint_min_max_target = joint_min_max_target
|
|
130
136
|
self.default_coords = self.child_link.copy_coords()
|
|
131
137
|
self._hooks = hooks
|
|
138
|
+
# The child link's (rotation, translation) arrays as this joint last
|
|
139
|
+
# set them, and the link they were set on (see _set_child_link).
|
|
140
|
+
self._child_written = None
|
|
141
|
+
|
|
142
|
+
def __getstate__(self):
|
|
143
|
+
# A copy (deepcopy, pickle) has its own, writeable, arrays: its joint
|
|
144
|
+
# takes its child link's pose as it is when it first moves.
|
|
145
|
+
state = self.__dict__.copy()
|
|
146
|
+
state['_child_written'] = None
|
|
147
|
+
return state
|
|
148
|
+
|
|
149
|
+
def _child_moved(self):
|
|
150
|
+
"""Has the child link been moved by something other than this joint
|
|
151
|
+
since the joint last set it (or has the joint never set it)?
|
|
152
|
+
|
|
153
|
+
Every Coordinates method or property setter that moves a link
|
|
154
|
+
(translate, rotate, newcoords, ``link.translation = v``, ...) assigns
|
|
155
|
+
it new arrays, so comparing the link's arrays with those the joint
|
|
156
|
+
wrote finds such a move.
|
|
157
|
+
"""
|
|
158
|
+
# (getattr: a joint pickled before it kept them has none)
|
|
159
|
+
w = getattr(self, '_child_written', None)
|
|
160
|
+
child = self.child_link
|
|
161
|
+
return (w is None or w[2] is not child
|
|
162
|
+
or child._rotation is not w[0]
|
|
163
|
+
or child._translation is not w[1])
|
|
164
|
+
|
|
165
|
+
def _set_child_link(self, rot, pos, moved, prev_rot, prev_pos):
|
|
166
|
+
"""Set the child link's local pose to default_coords moved by the
|
|
167
|
+
joint's motion now (rot, and pos in default_coords' axes, or None
|
|
168
|
+
for none), not turn it by the change of angle since the last pose:
|
|
169
|
+
turning by each change accumulates rounding (with float32 angles a
|
|
170
|
+
7-joint arm's links drift ~1e-5 m in 10k calls).
|
|
171
|
+
|
|
172
|
+
If the link was moved by something else since this joint last set it
|
|
173
|
+
(moved: see _child_moved), its pose now is taken as the joint's pose
|
|
174
|
+
at the previous angle (motion prev_rot, prev_pos): default_coords is
|
|
175
|
+
set back from it, so the move is kept, as it was when each change was
|
|
176
|
+
applied to the link.
|
|
177
|
+
|
|
178
|
+
The arrays written are read-only: a link's pose is changed through
|
|
179
|
+
Coordinates methods and property setters, not by writing into its
|
|
180
|
+
arrays (``link.translation[0] += 0.1`` raises ValueError once a joint
|
|
181
|
+
has set the link; such a write did not reach the link's descendants
|
|
182
|
+
either).
|
|
183
|
+
"""
|
|
184
|
+
child = self.child_link
|
|
185
|
+
d = self.default_coords
|
|
186
|
+
if moved:
|
|
187
|
+
d_rot = child._rotation if prev_rot is None \
|
|
188
|
+
else np.dot(child._rotation, prev_rot.T)
|
|
189
|
+
d_pos = child._translation if prev_pos is None \
|
|
190
|
+
else child._translation - np.dot(d_rot, prev_pos)
|
|
191
|
+
d.newcoords(d_rot, d_pos, check_validity=False)
|
|
192
|
+
t = type(child)
|
|
193
|
+
# (as CascadedCoords.rotate's fast path: set the arrays, then mark
|
|
194
|
+
# the descendants changed; else through newcoords)
|
|
195
|
+
fast = (getattr(child, '_hook', None) is None
|
|
196
|
+
and hasattr(child, '_descendants')
|
|
197
|
+
and t.newcoords is CascadedCoords.newcoords
|
|
198
|
+
and t.changed is CascadedCoords.changed)
|
|
199
|
+
keep_rot = keep_pos = None
|
|
200
|
+
else:
|
|
201
|
+
# what the joint does not move stays the array it wrote
|
|
202
|
+
fast = self._child_written[3]
|
|
203
|
+
keep_rot, keep_pos = child._rotation, child._translation
|
|
204
|
+
if rot is not None:
|
|
205
|
+
rot = np.dot(d._rotation, rot)
|
|
206
|
+
rot.setflags(write=False)
|
|
207
|
+
elif keep_rot is not None:
|
|
208
|
+
rot = keep_rot
|
|
209
|
+
else:
|
|
210
|
+
rot = d._rotation.copy()
|
|
211
|
+
rot.setflags(write=False)
|
|
212
|
+
if pos is not None:
|
|
213
|
+
pos = d._translation + np.dot(d._rotation, pos)
|
|
214
|
+
pos.setflags(write=False)
|
|
215
|
+
elif keep_pos is not None:
|
|
216
|
+
pos = keep_pos
|
|
217
|
+
else:
|
|
218
|
+
pos = d._translation.copy()
|
|
219
|
+
pos.setflags(write=False)
|
|
220
|
+
if fast:
|
|
221
|
+
child._rotation = rot
|
|
222
|
+
child._translation = pos
|
|
223
|
+
if not child._changed:
|
|
224
|
+
child._changed = True
|
|
225
|
+
stack = list(child._descendants)
|
|
226
|
+
while stack:
|
|
227
|
+
c = stack.pop()
|
|
228
|
+
if not c._changed:
|
|
229
|
+
c._changed = True
|
|
230
|
+
stack.extend(c._descendants)
|
|
231
|
+
else:
|
|
232
|
+
child.newcoords(rot, pos, check_validity=False)
|
|
233
|
+
# (newcoords keeps copies)
|
|
234
|
+
child._rotation.setflags(write=False)
|
|
235
|
+
child._translation.setflags(write=False)
|
|
236
|
+
self._child_written = (child._rotation, child._translation, child,
|
|
237
|
+
fast)
|
|
132
238
|
|
|
133
239
|
@property
|
|
134
240
|
def type(self):
|
|
@@ -542,18 +648,25 @@ class RotationalJoint(Joint):
|
|
|
542
648
|
if max_angle < min_angle:
|
|
543
649
|
v = 0.5 * (float(min_angle) + float(max_angle))
|
|
544
650
|
elif v > max_angle:
|
|
545
|
-
if not relative:
|
|
651
|
+
if not relative and v - max_angle > _limit_rounding:
|
|
546
652
|
logger.warning('%s :joint-angle(%s) violate max-angle(%s)', self, v, max_angle)
|
|
547
653
|
v = max_angle
|
|
548
654
|
elif v < min_angle:
|
|
549
|
-
if not relative:
|
|
655
|
+
if not relative and min_angle - v > _limit_rounding:
|
|
550
656
|
logger.warning('%s :joint-angle(%s) violate min-angle(%s)', self, v, min_angle)
|
|
551
657
|
v = min_angle
|
|
552
|
-
|
|
658
|
+
prev_angle = self._joint_angle
|
|
553
659
|
self._joint_angle = v
|
|
554
|
-
if
|
|
555
|
-
|
|
556
|
-
|
|
660
|
+
if v - prev_angle:
|
|
661
|
+
# (the previous angle's motion: only wanted when the link was
|
|
662
|
+
# moved by something else)
|
|
663
|
+
moved = self._child_moved()
|
|
664
|
+
self._set_child_link(
|
|
665
|
+
rotation_matrix(float(v), self.axis, skip_normalization=True),
|
|
666
|
+
None, moved,
|
|
667
|
+
rotation_matrix(float(prev_angle), self.axis,
|
|
668
|
+
skip_normalization=True) if moved else None,
|
|
669
|
+
None)
|
|
557
670
|
# Skip the for-loop setup when no hooks are registered.
|
|
558
671
|
if enable_hook and self._hooks:
|
|
559
672
|
for hook in self._hooks:
|
|
@@ -726,17 +839,20 @@ class LinearJoint(Joint):
|
|
|
726
839
|
max_angle = self.max_angle
|
|
727
840
|
min_angle = self.min_angle
|
|
728
841
|
if v > max_angle:
|
|
729
|
-
if not relative:
|
|
842
|
+
if not relative and v - max_angle > _limit_rounding:
|
|
730
843
|
logger.warning('%s :joint-angle(%s) violate max-angle(%s)', self, v, max_angle)
|
|
731
844
|
v = max_angle
|
|
732
845
|
elif v < min_angle:
|
|
733
|
-
if not relative:
|
|
846
|
+
if not relative and min_angle - v > _limit_rounding:
|
|
734
847
|
logger.warning('%s :joint-angle(%s) violate min-angle(%s)', self, v, min_angle)
|
|
735
848
|
v = min_angle
|
|
736
|
-
|
|
849
|
+
prev = self._joint_angle
|
|
737
850
|
self._joint_angle = v
|
|
738
|
-
if
|
|
739
|
-
|
|
851
|
+
if v - prev:
|
|
852
|
+
axis = np.asarray(self.axis, dtype=np.float64)
|
|
853
|
+
self._set_child_link(None, axis * float(v),
|
|
854
|
+
self._child_moved(), None,
|
|
855
|
+
axis * float(prev))
|
|
740
856
|
|
|
741
857
|
if enable_hook and self._hooks:
|
|
742
858
|
for hook in self._hooks:
|