Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rename cartesian_limits.yaml #1422

Merged
merged 3 commits into from
Jul 12, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions moveit2.repos
Original file line number Diff line number Diff line change
@@ -1 +1,10 @@
repositories:
# https://github.com/ros-planning/moveit_resources/pull/147
moveit_resources:
type: git
url: https://github.com/ros-planning/moveit_resources.git
version: ros2
ros2_control:
type: git
url: https://github.com/ros-controls/ros2_control.git
version: 2.12.0
31 changes: 19 additions & 12 deletions moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,8 @@
joint_limits.yaml -> Overriding position/velocity/acceleration limits from the URDF file
moveit_cpp.yaml -> MoveItCpp related parameters
*_planning.yaml -> planning pipelines parameters
cartesian_limits.yaml -> Pilz planner parameters
# TODO(JafarAbdi): Check to see if this is a good default value
robot_name_controllers.yaml -> trajectory execution manager's parameters
pilz_cartesian_limits.yaml -> Pilz planner parameters
moveit_controllers.yaml -> trajectory execution manager's parameters
...

Example:
Expand All @@ -32,7 +31,7 @@
moveit_configs.move_group_capabilities
moveit_configs.joint_limits
moveit_configs.moveit_cpp
moveit_configs.cartesian_limits
moveit_configs.pilz_cartesian_limits
# Or to get all the parameters as a dictionary
moveit_configs.to_dict()

Expand Down Expand Up @@ -112,7 +111,7 @@ class MoveItConfigs:
# A dictionary containing MoveItCpp related parameters.
moveit_cpp: Dict = field(default_factory=dict)
# A dictionary containing the cartesian limits for the Pilz planner.
cartesian_limits: Dict = field(default_factory=dict)
pilz_cartesian_limits: Dict = field(default_factory=dict)

def to_dict(self):
parameters = {}
Expand All @@ -125,7 +124,7 @@ def to_dict(self):
parameters.update(self.sensors_3d)
parameters.update(self.joint_limits)
parameters.update(self.moveit_cpp)
parameters.update(self.cartesian_limits)
parameters.update(self.pilz_cartesian_limits)
return parameters


Expand Down Expand Up @@ -425,17 +424,25 @@ def planning_pipelines(

return self

def cartesian_limits(self, file_path: Optional[str] = None):
def pilz_cartesian_limits(self, file_path: Optional[str] = None):
"""Load cartesian limits.

:param file_path: Absolute or relative path to the cartesian limits file (w.r.t. robot_name_moveit_config).
:return: Instance of MoveItConfigsBuilder with cartesian_limits loaded.
:return: Instance of MoveItConfigsBuilder with pilz_cartesian_limits loaded.
"""
self.__moveit_configs.cartesian_limits = {
deprecated_path = self._package_path / (
file_path or self.__config_dir_path / "cartesian_limits.yaml"
)
if deprecated_path.exists():
logging.warning(
f"\x1b[33;21mcartesian_limits.yaml is deprecated, please rename to pilz_cartesian_limits.yaml\x1b[0m"
)

self.__moveit_configs.pilz_cartesian_limits = {
self.__robot_description
+ "_planning": load_yaml(
self._package_path
/ (file_path or self.__config_dir_path / "cartesian_limits.yaml")
/ (file_path or self.__config_dir_path / "pilz_cartesian_limits.yaml")
)
}
return self
Expand Down Expand Up @@ -464,8 +471,8 @@ def to_moveit_configs(self):
# TODO(JafarAbdi): We should have a default moveit_cpp.yaml as port of a moveit config package
# if not self.__moveit_configs.moveit_cpp:
# self.moveit_cpp()
if not self.__moveit_configs.cartesian_limits:
self.cartesian_limits()
if not self.__moveit_configs.pilz_cartesian_limits:
self.pilz_cartesian_limits()
return self.__moveit_configs

def to_dict(self, include_moveit_configs: bool = True):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ def load_moveit_config():
robot_description_semantic_file = "config/prbt.srdf.xacro"
robot_description_kinematics_file = "config/kinematics.yaml"
joint_limits_file = "config/joint_limits.yaml"
cartesian_limits_file = "config/cartesian_limits.yaml"
pilz_cartesian_limits_file = "config/pilz_cartesian_limits.yaml"
# TODO(henningkayser): Switch to MoveItConfigsBuilder once #591 is merged
# return (
# MoveItConfigsBuilder(moveit_config_package_name)
Expand All @@ -62,7 +62,7 @@ def load_moveit_config():
# .robot_description_semantic(file_path=robot_description_semantic_file)
# .robot_description_kinematics(file_path=robot_description_kinematics_file)
# .joint_limits(file_path=joint_limits_file)
# .cartesian_limits(file_path=cartesian_limits_file)
# .pilz_cartesian_limits(file_path=pilz_cartesian_limits_file)
# .to_moveit_configs()
# )

Expand Down Expand Up @@ -98,7 +98,7 @@ def load_moveit_config():
configs.robot_description_planning = {
"robot_description_planning": {
**load_yaml(moveit_config_package_name, joint_limits_file),
**load_yaml(moveit_config_package_name, cartesian_limits_file),
**load_yaml(moveit_config_package_name, pilz_cartesian_limits_file),
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,13 @@ def generate_launch_description():
joint_limits_yaml = load_yaml(
"moveit_resources_prbt_moveit_config", "config/joint_limits.yaml"
)
cartesian_limits_yaml = load_yaml(
"moveit_resources_prbt_moveit_config", "config/cartesian_limits.yaml"
pilz_cartesian_limits_yaml = load_yaml(
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
"moveit_resources_prbt_moveit_config", "config/pilz_cartesian_limits.yaml"
)
robot_description_planning = {
"robot_description_planning": {
**joint_limits_yaml,
**cartesian_limits_yaml,
**pilz_cartesian_limits_yaml,
}
}
kinematics_yaml = load_yaml(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
<ros2_control name="${name}" type="system">
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>fake_components/GenericSystem</plugin>
<plugin>mock_components/GenericSystem</plugin>
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
<ros2_control name="${name}" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>fake_components/GenericSystem</plugin>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
[ROS2_CONTROL_JOINTS]
</ros2_control>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ enum InformationFields
};

static const std::string JOINT_LIMITS_FILE = "config/joint_limits.yaml";
static const std::string CARTESIAN_LIMITS_FILE = "config/cartesian_limits.yaml";
static const std::string CARTESIAN_LIMITS_FILE = "config/pilz_cartesian_limits.yaml";

class SRDFConfig : public SetupConfig
{
Expand Down
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
# Limits for the Pilz planner
cartesian_limits:
max_trans_vel: 1.0
max_trans_acc: 2.25
Expand Down