Skip to content

Commit

Permalink
Merge branch 'main' into smoothing-plugins
Browse files Browse the repository at this point in the history
  • Loading branch information
sea-bass authored Oct 24, 2023
2 parents 6b5ee5c + 19c58b8 commit 5250477
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -96,8 +96,8 @@ class CollisionEnvBullet : public CollisionEnv
void updatedPaddingOrScaling(const std::vector<std::string>& links) override;

/** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */
void addAttachedOjects(const moveit::core::RobotState& state,
std::vector<collision_detection_bullet::CollisionObjectWrapperPtr>& cows) const;
void addAttachedObjects(const moveit::core::RobotState& state,
std::vector<collision_detection_bullet::CollisionObjectWrapperPtr>& cows) const;

/** \brief Bundles the different checkSelfCollision functions into a single function */
void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ void CollisionEnvBullet::checkSelfCollisionHelper(const CollisionRequest& req, C
std::lock_guard<std::mutex> guard(collision_env_mutex_);

std::vector<collision_detection_bullet::CollisionObjectWrapperPtr> cows;
addAttachedOjects(state, cows);
addAttachedObjects(state, cows);

if (req.distance)
{
Expand Down Expand Up @@ -187,7 +187,7 @@ void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req,
}

std::vector<collision_detection_bullet::CollisionObjectWrapperPtr> attached_cows;
addAttachedOjects(state, attached_cows);
addAttachedObjects(state, attached_cows);
updateTransformsFromState(state, manager_);

for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows)
Expand All @@ -213,7 +213,7 @@ void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& re
std::lock_guard<std::mutex> guard(collision_env_mutex_);

std::vector<collision_detection_bullet::CollisionObjectWrapperPtr> attached_cows;
addAttachedOjects(state1, attached_cows);
addAttachedObjects(state1, attached_cows);

for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows)
{
Expand Down Expand Up @@ -331,8 +331,8 @@ void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Ac
}
}

void CollisionEnvBullet::addAttachedOjects(const moveit::core::RobotState& state,
std::vector<collision_detection_bullet::CollisionObjectWrapperPtr>& cows) const
void CollisionEnvBullet::addAttachedObjects(const moveit::core::RobotState& state,
std::vector<collision_detection_bullet::CollisionObjectWrapperPtr>& cows) const
{
std::vector<const moveit::core::AttachedBody*> attached_bodies;
state.getAttachedBodies(attached_bodies);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ void init_robot_trajectory(py::module& m)
py::arg("velocity_scaling_factor"), py::arg("acceleration_scaling_factor"), py::kw_only(),
py::arg("path_tolerance") = 0.1, py::arg("resample_dt") = 0.1, py::arg("min_angle_change") = 0.001,
R"(
Adds time parameterization to the trajectory using the Time-Optimal Trajectory Generation (TOTG) algorithm.
Adds time parameterization to the trajectory using the Time-Optimal Trajectory Generation (TOTG) algorithm.
Args:
velocity_scaling_factor (float): The velocity scaling factor.
acceleration_scaling_factor (float): The acceleration scaling factor.
Expand All @@ -154,7 +154,7 @@ void init_robot_trajectory(py::module& m)
py::arg("acceleration_scaling_factor"), py::kw_only(), py::arg("mitigate_overshoot") = false,
py::arg("overshoot_threshold") = 0.01,
R"(
Applies Ruckig smoothing to the trajectory.
Applies Ruckig smoothing to the trajectory.
Args:
velocity_scaling_factor (float): The velocity scaling factor.
acceleration_scaling_factor (float): The acceleration scaling factor.
Expand All @@ -168,8 +168,7 @@ void init_robot_trajectory(py::module& m)
py::arg("joint_filter") = std::vector<std::string>(),
R"(
Get the trajectory as a moveit_msgs.msg.RobotTrajectory message.
Returns:
Returns:
moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message.
)")
.def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg,
Expand Down

0 comments on commit 5250477

Please sign in to comment.