SMACC2
cb_move_cartesian_relative.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
22
24
26{
28
29CbMoveCartesianRelative::CbMoveCartesianRelative(geometry_msgs::msg::Vector3 offset)
30: offset_(offset)
31{
32}
33
35{
37
38 if (this->group_)
39 {
40 moveit::planning_interface::MoveGroupInterface move_group(
41 getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
42 this->moveRelativeCartesian(&move_group, offset_);
43 }
44 else
45 {
47 }
48}
49
51
52// keeps the end efector orientation fixed
54 moveit::planning_interface::MoveGroupInterface * movegroupClient,
55 geometry_msgs::msg::Vector3 & offset)
56{
57 std::vector<geometry_msgs::msg::Pose> waypoints;
58
59 // this one was working fine but the issue is that for relative motions it grows up on ABORT-State-Loop pattern.
60 // But, we need current pose because maybe setCurrentPose was not set by previous behaviors. The only solution would be
61 // distinguishing between the execution error and the planning error with no state change
62 //auto referenceStartPose = movegroupClient->getPoseTarget();
63 auto referenceStartPose = movegroupClient->getCurrentPose();
64 movegroupClient->setPoseReferenceFrame(referenceStartPose.header.frame_id);
65
66 RCLCPP_INFO_STREAM(
67 getLogger(), "[CbMoveCartesianRelative] RELATIVE MOTION, SOURCE POSE: " << referenceStartPose);
68 RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Offset: " << offset);
69
70 waypoints.push_back(referenceStartPose.pose); // up and out
71
72 auto endPose = referenceStartPose.pose;
73
74 endPose.position.x += offset.x;
75 endPose.position.y += offset.y;
76 endPose.position.z += offset.z;
77
78 RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] DESTINY POSE: " << endPose);
79
80 // target_pose2.position.x -= 0.15;
81 waypoints.push_back(endPose); // left
82
83 movegroupClient->setPoseTarget(endPose);
84
85 float scalinf = 0.5;
86 if (scalingFactor_) scalinf = *scalingFactor_;
87
88 RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Using scaling factor: " << scalinf);
89 movegroupClient->setMaxVelocityScalingFactor(scalinf);
90
91 moveit_msgs::msg::RobotTrajectory trajectory;
92 double fraction = movegroupClient->computeCartesianPath(
93 waypoints,
94 0.01, // eef_step
95 0.00, // jump_threshold
96 trajectory);
97
98 moveit::planning_interface::MoveItErrorCode behaviorResult;
99 if (fraction != 1.0 || fraction == -1)
100 {
101 RCLCPP_WARN_STREAM(
102 getLogger(),
103 "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje. Execution skipped "
104 "because not 100% of cartesian motion: "
105 << fraction * 100 << "%");
106 behaviorResult = moveit::planning_interface::MoveItErrorCode::PLANNING_FAILED;
107 }
108 else
109 {
110 RCLCPP_INFO_STREAM(
111 getLogger(),
112 "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje: " << fraction);
113
114 moveit::planning_interface::MoveGroupInterface::Plan grasp_pose_plan;
115
116 // grasp_pose_plan.start_state_ = *(moveGroupInterface.getCurrentState());
117 grasp_pose_plan.trajectory_ = trajectory;
118 behaviorResult = movegroupClient->execute(grasp_pose_plan);
119 }
120
121 if (behaviorResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
122 {
123 RCLCPP_INFO(
124 getLogger(), "[CbMoveCartesianRelative] relative motion execution succeeded: fraction %lf.",
125 fraction);
127 this->postSuccessEvent();
128 }
129 else
130 {
131 movegroupClient->setPoseTarget(
132 referenceStartPose); // undo changes since we did not executed the motion
133 RCLCPP_INFO(
134 getLogger(), "[CbMoveCartesianRelative] relative motion execution failed: fraction %lf.",
135 fraction);
137 this->postFailureEvent();
138 }
139}
140} // namespace cl_move_group_interface
void moveRelativeCartesian(moveit::planning_interface::MoveGroupInterface *movegroupClient, geometry_msgs::msg::Vector3 &offset)
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Node::SharedPtr getNode()
void requiresClient(SmaccClientType *&storage)