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

Allow Scaling Collision Meshes #62

Merged
Changes from 1 commit
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
Prev Previous commit
Next Next commit
Ran tests with example
amalnanavati committed Apr 23, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
commit 6a056200c4d2820cfcb4cecf34c3b019b025a9b0
9 changes: 7 additions & 2 deletions examples/ex_collision_mesh.py
Original file line number Diff line number Diff line change
@@ -2,7 +2,7 @@
"""
Example of adding and removing a collision object with a mesh geometry.
Note: Python module `trimesh` is required for this example (`pip install trimesh`).
- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.7071, 0.7071]"
- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.7071, 0.7071]" -p scale:="[1.0, 1.0, 1.0]"
- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="move" -p position:="[0.2, 0.0, 0.2]"
- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p filepath:="./my_favourity_mesh.stl"
- ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="remove"
@@ -40,6 +40,7 @@ def main():
)
node.declare_parameter("position", [0.5, 0.0, 0.5])
node.declare_parameter("quat_xyzw", [0.0, 0.0, -0.7071, 0.7071])
node.declare_parameter("scale", [1.0, 1.0, 1.0])

# Create callback group that allows execution of callbacks in parallel without restrictions
callback_group = ReentrantCallbackGroup()
@@ -66,10 +67,13 @@ def main():
action = node.get_parameter("action").get_parameter_value().string_value
position = node.get_parameter("position").get_parameter_value().double_array_value
quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
scale = node.get_parameter("scale").get_parameter_value().double_array_value

# Use the default example mesh if invalid
if not filepath:
node.get_logger().info(f"Using the default example mesh file")
node.get_logger().info(
f"Using the default example mesh file {DEFAULT_EXAMPLE_MESH}"
)
filepath = DEFAULT_EXAMPLE_MESH

# Make sure the mesh file exists
@@ -92,6 +96,7 @@ def main():
id=object_id,
position=position,
quat_xyzw=quat_xyzw,
scale=scale,
)
elif action == "remove":
# Remove collision mesh
4 changes: 2 additions & 2 deletions pymoveit2/moveit2.py
Original file line number Diff line number Diff line change
@@ -3,6 +3,7 @@
from enum import Enum
from typing import List, Optional, Tuple, Union

import numpy as np
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
from moveit_msgs.action import ExecuteTrajectory, MoveGroup
@@ -24,7 +25,6 @@
GetPositionFK,
GetPositionIK,
)
import numpy as np
from rclpy.action import ActionClient
from rclpy.callback_groups import CallbackGroup
from rclpy.node import Node
@@ -1687,7 +1687,7 @@ def add_collision_mesh(
if isinstance(scale, float):
scale = (scale, scale, scale)
transform = np.eye(4)
transform.fill_diagonal(scale)
np.fill_diagonal(transform, scale)
mesh.apply_transform(transform)

msg.meshes.append(