Skip to content

Commit e9d6289

Browse files
Levi ArmstrongLevi-Armstrong
authored andcommitted
Update motion planners to account for Joint and State Waypoints unordered joints relative to kinematics
1 parent d39f7fa commit e9d6289

File tree

14 files changed

+324
-35
lines changed

14 files changed

+324
-35
lines changed

tesseract/tesseract_common/include/tesseract_common/manipulator_info.h

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -48,20 +48,20 @@ class ToolCenterPoint
4848
/**
4949
* @brief Tool Center Point Defined by name
5050
* @param name The tool center point name
51-
* @param external The external TCP is used when the robot is holding the part verus the tool.
52-
* @note External should also be set to true your kinematic object which includes say a robot and
53-
* positioner, and the positioner has the tool and the robot is holding the part. Basically
51+
* @param external The external TCP is used when the robot is holding the part versus the tool.
52+
* @note External should also be set to true when your kinematic object includes a robot and
53+
* positioner, where the positioner has the tool and the robot is holding the part. Basically
5454
* anytime the tool is not attached to the tip link of the kinematic chain.
5555
*/
5656
ToolCenterPoint(const std::string& name, bool external = false);
5757

5858
/**
5959
* @brief Tool Center Point Defined by transform
6060
* @param transform The tool center point transform
61-
* @param external The external TCP is used when the robot is holding the part verus the tool.
61+
* @param external The external TCP is used when the robot is holding the part versus the tool.
6262
* @param external_frame If empty assumed relative to world, otherwise the provided tcp is relative to this link.
63-
* @note External should also be set to true your kinematic object which includes say a robot and
64-
* positioner, and the positioner has the tool and the robot is holding the part. Basically
63+
* @note External should also be set to true when your kinematic object includes a robot and
64+
* positioner, where the positioner has the tool and the robot is holding the part. Basically
6565
* anytime the tool is not attached to the tip link of the kinematic chain.
6666
*/
6767
ToolCenterPoint(const Eigen::Isometry3d& transform, bool external = false, std::string external_frame = "");
@@ -86,9 +86,9 @@ class ToolCenterPoint
8686

8787
/**
8888
* @brief Check if tool center point is and external tcp which mean it is not defined
89-
* @details The external TCP is used when the robot is holding the part verus the tool.
90-
* External should also be set to true your kinematic object which includes say a robot and
91-
* positioner, and the positioner has the tool and the robot is holding the part. Basically
89+
* @details The external TCP is used when the robot is holding the part versus the tool.
90+
* External should also be set to true when your kinematic object includes a robot and
91+
* positioner, where the positioner has the tool and the robot is holding the part. Basically
9292
* anytime the tool is not attached to the tip link of the kinematic chain.
9393
* @return True if and external TCP, otherwise the false
9494
*/
@@ -137,9 +137,9 @@ class ToolCenterPoint
137137
Eigen::Isometry3d transform_;
138138

139139
/**
140-
* @brief The external TCP is used when the robot is holding the part verus the tool.
141-
* @details External should also be set to true your kinematic object which includes say a robot and
142-
* positioner, and the positioner has the tool and the robot is holding the part. Basically
140+
* @brief The external TCP is used when the robot is holding the part versus the tool.
141+
* @details External should also be set to true when your kinematic object includes a robot and
142+
* positioner, where the positioner has the tool and the robot is holding the part. Basically
143143
* anytime the tool is not attached to the tip link of the kinematic chain.
144144
*/
145145
bool external_{ false };

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,34 @@ const std::vector<std::string>& getJointNames(const Waypoint& waypoint);
6767
*/
6868
Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
6969

70+
/**
71+
* @brief Format the waypoints joint ordered by the provided joint names
72+
*
73+
* Throws if waypoint does not directly contain that information
74+
*
75+
* Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
76+
* the program all match.
77+
*
78+
* @param joint_names The joint names defining the order desired
79+
* @param waypoint The waypoint to format
80+
* @return True if formating was required, otherwise false.
81+
*/
82+
bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint);
83+
84+
/**
85+
* @brief Check the waypoints joint order against the provided joint names
86+
*
87+
* Throws if waypoint does not directly contain that information
88+
*
89+
* Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
90+
* the program all match.
91+
*
92+
* @param joint_names The joint names defining the order desired
93+
* @param waypoint The waypoint to check format
94+
* @return True if waypoint format is correct, otherwise false.
95+
*/
96+
bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
97+
7098
/**
7199
* @brief Set the joint position for waypoints that contain that information
72100
* @param waypoint Waypoint to set

tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp

Lines changed: 60 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
6666
if (isJointWaypoint(waypoint))
6767
{
6868
const auto* jwp = waypoint.cast_const<JointWaypoint>();
69-
jv = *jwp;
69+
jv = jwp->waypoint;
7070
jn = jwp->joint_names;
7171
}
7272
else if (isStateWaypoint(waypoint))
@@ -80,7 +80,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
8080
throw std::runtime_error("Unsupported waypoint type.");
8181
}
8282

83-
if (jn.size() == joint_names.size())
83+
if (jn.size() != joint_names.size())
8484
throw std::runtime_error("Joint name sizes do not match!");
8585

8686
if (joint_names == jn)
@@ -103,6 +103,64 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
103103
return output;
104104
}
105105

106+
bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint)
107+
{
108+
Eigen::VectorXd* jv;
109+
std::vector<std::string>* jn;
110+
if (isJointWaypoint(waypoint))
111+
{
112+
auto* jwp = waypoint.cast<JointWaypoint>();
113+
jv = &(jwp->waypoint);
114+
jn = &(jwp->joint_names);
115+
}
116+
else if (isStateWaypoint(waypoint))
117+
{
118+
auto* swp = waypoint.cast<StateWaypoint>();
119+
jv = &(swp->position);
120+
jn = &(swp->joint_names);
121+
}
122+
else
123+
{
124+
throw std::runtime_error("Unsupported waypoint type.");
125+
}
126+
127+
if (jn->size() != joint_names.size())
128+
throw std::runtime_error("Joint name sizes do not match!");
129+
130+
if (joint_names == *jn)
131+
return false;
132+
133+
Eigen::VectorXd output = *jv;
134+
for (std::size_t i = 0; i < joint_names.size(); ++i)
135+
{
136+
if (joint_names[i] == (*jn)[i])
137+
continue;
138+
139+
auto it = std::find(jn->begin(), jn->end(), joint_names[i]);
140+
if (it == jn->end())
141+
throw std::runtime_error("Joint names do not match!");
142+
143+
long idx = std::distance(jn->begin(), it);
144+
output(static_cast<long>(i)) = (*jv)(static_cast<long>(idx));
145+
}
146+
147+
*jn = joint_names;
148+
*jv = output;
149+
150+
return true;
151+
}
152+
153+
bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint)
154+
{
155+
if (isJointWaypoint(waypoint))
156+
return (joint_names == waypoint.cast_const<JointWaypoint>()->joint_names);
157+
158+
if (isStateWaypoint(waypoint))
159+
return (joint_names == waypoint.cast_const<StateWaypoint>()->joint_names);
160+
161+
throw std::runtime_error("Unsupported waypoint type.");
162+
}
163+
106164
bool setJointPosition(Waypoint& waypoint, const Eigen::Ref<const Eigen::VectorXd>& position)
107165
{
108166
if (isJointWaypoint(waypoint))

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -183,6 +183,14 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
183183
CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_instructions,
184184
const tesseract_environment::Environment& env);
185185

186+
/**
187+
* @brief This formats the joint and state waypoints to align with the kinematics object
188+
* @param composite_instructions The input program to format
189+
* @param env The environment information
190+
* @return True if the program required formating.
191+
*/
192+
bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env);
193+
186194
} // namespace tesseract_planning
187195

188196
#endif // TESSERACT_PLANNING_UTILS_H

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp

Lines changed: 15 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,10 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
139139
auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
140140
auto instructions_flattened = flattenProgram(request.instructions);
141141

142+
// Check inverse and forward kinematic objects
143+
if (problem->manip_fwd_kin->getJointNames() != problem->manip_inv_kin->getJointNames())
144+
throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
145+
142146
// Loop over the flattened results and add them to response if the input was a plan instruction
143147
Eigen::Index dof = problem->manip_fwd_kin->numJoints();
144148
Eigen::Index result_index = 0;
@@ -157,7 +161,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
157161
auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
158162

159163
Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++, dof);
160-
move_instruction->getWaypoint().cast<StateWaypoint>()->position = temp.template cast<double>();
164+
auto* swp = move_instruction->getWaypoint().cast<StateWaypoint>();
165+
swp->position = temp.template cast<double>();
166+
assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
161167
}
162168
else if (plan_instruction->isLinear())
163169
{
@@ -168,8 +174,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
168174
{
169175
Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++,
170176
dof);
171-
instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
172-
temp.template cast<double>();
177+
auto* swp = instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
178+
swp->position = temp.template cast<double>();
179+
assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
173180
}
174181
}
175182
else if (plan_instruction->isFreespace())
@@ -190,8 +197,11 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
190197

191198
assert(temp.cols() == static_cast<long>(move_instructions->size()) + 1);
192199
for (std::size_t i = 0; i < move_instructions->size(); ++i)
193-
(*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
194-
temp.col(static_cast<long>(i) + 1);
200+
{
201+
auto* swp = (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
202+
swp->position = temp.col(static_cast<long>(i) + 1);
203+
assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
204+
}
195205
}
196206
else
197207
{

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -134,6 +134,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
134134
}
135135
else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
136136
{
137+
assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
137138
const Eigen::VectorXd& position = getJointPosition(start_waypoint);
138139
cur_plan_profile->apply(*prob, position, *start_instruction, composite_mi, active_links, index);
139140
}
@@ -187,6 +188,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
187188
}
188189
else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
189190
{
191+
assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
190192
const Eigen::VectorXd& position = getJointPosition(start_waypoint);
191193
if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
192194
throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -214,6 +216,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
214216
}
215217
else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
216218
{
219+
assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
217220
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
218221
Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
219222
if (!prob->manip_fwd_kin->calcFwdKin(cur_pose, cur_position))
@@ -228,6 +231,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
228231
}
229232
else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
230233
{
234+
assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
231235
const Eigen::VectorXd& position = getJointPosition(start_waypoint);
232236
if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
233237
throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -262,6 +266,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
262266
{
263267
if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
264268
{
269+
assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
265270
const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
266271

267272
// Descartes does not support freespace so it will only include the plan instruction state, then in

0 commit comments

Comments
 (0)