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

Add Path Constraints #42

Merged
merged 11 commits into from
Jan 5, 2024
Prev Previous commit
Pre-commit formatting fix
amalnanavati committed Jan 2, 2024
commit 0c319ef3b3caff056e43c0e5f84b74fddec18048
46 changes: 27 additions & 19 deletions examples/ex_orientation_path_constraint.py
Original file line number Diff line number Diff line change
@@ -24,15 +24,7 @@ def main():
# Declare parameter for joint positions
node.declare_parameter(
"initial_joint_positions",
[
1.57,
-1.57,
0.0,
-1.57,
0.0,
1.57,
0.7854
],
[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854],
)
node.declare_parameter(
"goal_joint_positions",
@@ -48,7 +40,7 @@ def main():
)
node.declare_parameter("use_orientation_constraint", True)
node.declare_parameter(
"orientation_constraint_quaternion",
"orientation_constraint_quaternion",
[
0.5,
-0.5,
@@ -57,7 +49,7 @@ def main():
],
)
node.declare_parameter(
"orientation_constraint_tolerance",
"orientation_constraint_tolerance",
[
3.14159,
0.5,
@@ -92,26 +84,40 @@ def main():

# Get parameters
initial_joint_positions = (
node.get_parameter("initial_joint_positions").get_parameter_value().double_array_value
node.get_parameter("initial_joint_positions")
.get_parameter_value()
.double_array_value
)
goal_joint_positions = (
node.get_parameter("goal_joint_positions").get_parameter_value().double_array_value
node.get_parameter("goal_joint_positions")
.get_parameter_value()
.double_array_value
)
use_orientation_constraint = (
node.get_parameter("use_orientation_constraint").get_parameter_value().bool_value
node.get_parameter("use_orientation_constraint")
.get_parameter_value()
.bool_value
)
orientation_constraint_quaternion = (
node.get_parameter("orientation_constraint_quaternion").get_parameter_value().double_array_value
node.get_parameter("orientation_constraint_quaternion")
.get_parameter_value()
.double_array_value
)
orientation_constraint_tolerance = (
node.get_parameter("orientation_constraint_tolerance").get_parameter_value().double_array_value
node.get_parameter("orientation_constraint_tolerance")
.get_parameter_value()
.double_array_value
)
orientation_constraint_parameterization = (
node.get_parameter("orientation_constraint_parameterization").get_parameter_value().integer_value
node.get_parameter("orientation_constraint_parameterization")
.get_parameter_value()
.integer_value
)

# Move to initial joint configuration
node.get_logger().info(f"Moving to {{joint_positions: {list(initial_joint_positions)}}}")
node.get_logger().info(
f"Moving to {{joint_positions: {list(initial_joint_positions)}}}"
)
moveit2.move_to_configuration(initial_joint_positions)
moveit2.wait_until_executed()

@@ -125,7 +131,9 @@ def main():
)

# Move to goal joint configuration
node.get_logger().info(f"Moving to {{joint_positions: {list(goal_joint_positions)}}}")
node.get_logger().info(
f"Moving to {{joint_positions: {list(goal_joint_positions)}}}"
)
moveit2.move_to_configuration(goal_joint_positions)
moveit2.wait_until_executed()