Skip to content

Commit 12bed9c

Browse files
rjoomenrjoomen
committed
Rename SimplePlannerFixedSizeAssignPlanProfile to SimplePlannerFixedSizeAssignNoIKPlanProfile and add SimplePlannerFixedSizeAssignPlanProfile (with IK)
1 parent 8d4374b commit 12bed9c

File tree

2 files changed

+786
-0
lines changed

2 files changed

+786
-0
lines changed

‎tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_plan_unit.cpp‎

Lines changed: 202 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,202 @@
1+
/**
2+
* @file simple_planner_fixed_size_assign_plan_unit.cpp
3+
* @brief
4+
*
5+
* @author Levi Armstrong
6+
* @date July 28, 2020
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2020, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#include <tesseract_common/macros.h>
27+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
28+
#include <gtest/gtest.h>
29+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
30+
31+
#include <tesseract_common/types.h>
32+
#include <tesseract_kinematics/core/joint_group.h>
33+
#include <tesseract_scene_graph/scene_state.h>
34+
#include <tesseract_environment/environment.h>
35+
#include <tesseract_motion_planners/core/types.h>
36+
#include <tesseract_motion_planners/simple/simple_motion_planner.h>
37+
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h>
38+
#include <tesseract_command_language/joint_waypoint.h>
39+
#include <tesseract_command_language/cartesian_waypoint.h>
40+
#include <tesseract_command_language/move_instruction.h>
41+
#include <tesseract_common/resource_locator.h>
42+
43+
using namespace tesseract_environment;
44+
using namespace tesseract_planning;
45+
46+
class TesseractPlanningSimplePlannerFixedSizeAssignPlanProfileUnit : public ::testing::Test
47+
{
48+
protected:
49+
Environment::Ptr env_;
50+
tesseract_common::ManipulatorInfo manip_info_;
51+
std::vector<std::string> joint_names_;
52+
53+
void SetUp() override
54+
{
55+
auto locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
56+
Environment::Ptr env = std::make_shared<Environment>();
57+
tesseract_common::fs::path urdf_path(
58+
locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath());
59+
tesseract_common::fs::path srdf_path(
60+
locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath());
61+
EXPECT_TRUE(env->init(urdf_path, srdf_path, locator));
62+
env_ = env;
63+
64+
manip_info_.tcp_frame = "tool0";
65+
manip_info_.working_frame = "base_link";
66+
manip_info_.manipulator = "manipulator";
67+
joint_names_ = env_->getJointGroup("manipulator")->getJointNames();
68+
}
69+
};
70+
71+
TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPlanProfileUnit, JointCartesian_AssignJointPosition) // NOLINT
72+
{
73+
PlannerRequest request;
74+
request.env = env_;
75+
request.env_state = env_->getState();
76+
JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) };
77+
MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
78+
MoveInstruction instr1_seed{ instr1 };
79+
instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_)));
80+
81+
CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) };
82+
MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
83+
84+
InstructionPoly instr3;
85+
86+
SimplePlannerFixedSizeAssignPlanProfile profile(10, 10);
87+
std::vector<MoveInstructionPoly> move_instructions =
88+
profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo());
89+
EXPECT_EQ(move_instructions.size(), 10);
90+
for (std::size_t i = 0; i < move_instructions.size() - 1; ++i)
91+
{
92+
const MoveInstructionPoly& mi = move_instructions.at(i);
93+
EXPECT_TRUE(mi.getWaypoint().isJointWaypoint());
94+
EXPECT_TRUE(wp1.getPosition().isApprox(mi.getWaypoint().as<JointWaypointPoly>().getPosition(), 1e-5));
95+
EXPECT_FALSE(mi.getWaypoint().as<JointWaypointPoly>().isConstrained());
96+
if (instr2.getPathProfile().empty())
97+
{
98+
EXPECT_EQ(mi.getProfile(), instr2.getProfile());
99+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
100+
}
101+
else
102+
{
103+
EXPECT_EQ(mi.getProfile(), instr2.getPathProfile());
104+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
105+
}
106+
}
107+
const MoveInstructionPoly& mi = move_instructions.back();
108+
EXPECT_TRUE(wp1.getPosition().isApprox(mi.getWaypoint().as<CartesianWaypointPoly>().getSeed().position, 1e-5));
109+
EXPECT_EQ(mi.getProfile(), instr2.getProfile());
110+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
111+
}
112+
113+
TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPlanProfileUnit, CartesianJoint_AssignJointPosition) // NOLINT
114+
{
115+
PlannerRequest request;
116+
request.env = env_;
117+
request.env_state = env_->getState();
118+
CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) };
119+
MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
120+
MoveInstruction instr1_seed{ instr1 };
121+
instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_)));
122+
123+
JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) };
124+
MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
125+
126+
InstructionPoly instr3;
127+
128+
SimplePlannerFixedSizeAssignPlanProfile profile(10, 10);
129+
std::vector<MoveInstructionPoly> move_instructions =
130+
profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo());
131+
EXPECT_EQ(move_instructions.size(), 10);
132+
for (std::size_t i = 0; i < move_instructions.size() - 1; ++i)
133+
{
134+
const MoveInstructionPoly& mi = move_instructions.at(i);
135+
EXPECT_TRUE(mi.getWaypoint().isJointWaypoint());
136+
EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as<JointWaypointPoly>().getPosition(), 1e-5));
137+
if (instr2.getPathProfile().empty())
138+
{
139+
EXPECT_EQ(mi.getProfile(), instr2.getProfile());
140+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
141+
}
142+
else
143+
{
144+
EXPECT_EQ(mi.getProfile(), instr2.getPathProfile());
145+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
146+
}
147+
}
148+
const MoveInstructionPoly& mi = move_instructions.back();
149+
EXPECT_TRUE(wp2.getPosition().isApprox(mi.getWaypoint().as<JointWaypointPoly>().getPosition(), 1e-5));
150+
EXPECT_EQ(mi.getProfile(), instr2.getProfile());
151+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
152+
}
153+
154+
TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPlanProfileUnit, CartesianCartesian_AssignJointPosition) // NOLINT
155+
{
156+
PlannerRequest request;
157+
request.env = env_;
158+
request.env_state = env_->getState();
159+
CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) };
160+
MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
161+
MoveInstruction instr1_seed{ instr1 };
162+
instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_)));
163+
164+
CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) };
165+
MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
166+
167+
InstructionPoly instr3;
168+
169+
SimplePlannerFixedSizeAssignPlanProfile profile(10, 10);
170+
std::vector<MoveInstructionPoly> move_instructions =
171+
profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo());
172+
auto fwd_kin = env_->getJointGroup(manip_info_.manipulator);
173+
Eigen::VectorXd position = request.env_state.getJointValues(fwd_kin->getJointNames());
174+
EXPECT_EQ(move_instructions.size(), 10);
175+
for (std::size_t i = 0; i < move_instructions.size() - 1; ++i)
176+
{
177+
const MoveInstructionPoly& mi = move_instructions.at(i);
178+
EXPECT_TRUE(mi.getWaypoint().isJointWaypoint());
179+
EXPECT_TRUE(position.isApprox(mi.getWaypoint().as<JointWaypointPoly>().getPosition(), 1e-5));
180+
if (instr2.getPathProfile().empty())
181+
{
182+
EXPECT_EQ(mi.getProfile(), instr2.getProfile());
183+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
184+
}
185+
else
186+
{
187+
EXPECT_EQ(mi.getProfile(), instr2.getPathProfile());
188+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
189+
}
190+
}
191+
const MoveInstructionPoly& mi = move_instructions.back();
192+
EXPECT_TRUE(position.isApprox(mi.getWaypoint().as<CartesianWaypointPoly>().getSeed().position, 1e-5));
193+
EXPECT_EQ(mi.getProfile(), instr2.getProfile());
194+
EXPECT_EQ(mi.getPathProfile(), instr2.getPathProfile());
195+
}
196+
197+
int main(int argc, char** argv)
198+
{
199+
testing::InitGoogleTest(&argc, argv);
200+
201+
return RUN_ALL_TESTS();
202+
}

0 commit comments

Comments
 (0)