mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge PR #380: Fix Cartesian interpolation
Correctly consider an offset transform from link to reference frame, such that rotations w.r.t. the reference frame don't move its origin.
This commit is contained in:
commit
f88e25fe74
@ -66,8 +66,8 @@ public:
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||
};
|
||||
} // namespace solvers
|
||||
|
||||
@ -63,8 +63,8 @@ public:
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||
};
|
||||
} // namespace solvers
|
||||
|
||||
@ -84,8 +84,8 @@ public:
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||
|
||||
protected:
|
||||
|
||||
@ -84,9 +84,10 @@ public:
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||
|
||||
/// plan trajectory from current robot state to Cartesian target
|
||||
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
|
||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||
};
|
||||
|
||||
@ -70,11 +70,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
}
|
||||
|
||||
// reach pose of forward kinematics
|
||||
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints);
|
||||
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
|
||||
timeout, result, path_constraints);
|
||||
}
|
||||
|
||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
@ -96,7 +98,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
||||
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
||||
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
|
||||
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
|
||||
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), offset);
|
||||
|
||||
assert(!trajectory.empty()); // there should be at least the start state
|
||||
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
||||
|
||||
@ -99,9 +99,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
||||
}
|
||||
|
||||
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
|
||||
@ -117,7 +117,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
||||
return to->isStateValid(*robot_state, constraints, jmg->getName());
|
||||
} };
|
||||
|
||||
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
|
||||
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
|
||||
// TODO(v4hn): planners need a way to add feedback to failing plans
|
||||
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
|
||||
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
|
||||
|
||||
@ -172,8 +172,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
}
|
||||
|
||||
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
moveit_msgs::MotionPlanRequest req;
|
||||
@ -181,7 +182,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
|
||||
|
||||
geometry_msgs::PoseStamped target;
|
||||
target.header.frame_id = from->getPlanningFrame();
|
||||
target.pose = tf2::toMsg(target_eigen);
|
||||
target.pose = tf2::toMsg(target_eigen * offset.inverse());
|
||||
|
||||
req.goal_constraints.resize(1);
|
||||
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
||||
|
||||
@ -104,8 +104,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
|
||||
return false;
|
||||
}
|
||||
|
||||
// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance
|
||||
static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success,
|
||||
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose,
|
||||
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose,
|
||||
const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
|
||||
double linear_norm = linear.norm();
|
||||
|
||||
@ -114,7 +115,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
|
||||
auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
|
||||
|
||||
// link position before planning; reached link position after planning; target link position
|
||||
Eigen::Vector3d pos_link = link_pose.translation();
|
||||
Eigen::Vector3d pos_start = start_pose.translation();
|
||||
Eigen::Vector3d pos_reached = reached_pose.translation();
|
||||
Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0);
|
||||
|
||||
@ -124,7 +125,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
|
||||
if (dir == Interface::FORWARD) {
|
||||
if (success) {
|
||||
// valid part: green arrow
|
||||
rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm);
|
||||
rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
markers.push_back(m);
|
||||
} else {
|
||||
@ -139,14 +140,14 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
|
||||
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
// position half-way between pos_link and pos_reached
|
||||
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) });
|
||||
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) });
|
||||
m.pose.orientation = tf2::toMsg(quat_cylinder);
|
||||
markers.push_back(m);
|
||||
}
|
||||
} else {
|
||||
// valid part: green arrow
|
||||
// head length according to above comment
|
||||
rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm);
|
||||
rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
markers.push_back(m);
|
||||
if (!success) {
|
||||
@ -203,10 +204,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
Eigen::Vector3d linear; // linear translation
|
||||
Eigen::Vector3d angular; // angular rotation
|
||||
double linear_norm = 0.0, angular_norm = 0.0;
|
||||
|
||||
Eigen::Isometry3d target_eigen;
|
||||
Eigen::Isometry3d link_pose =
|
||||
scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
|
||||
|
||||
try { // try to extract Twist
|
||||
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(direction);
|
||||
@ -240,13 +238,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
angular *= -1.0;
|
||||
}
|
||||
|
||||
// compute absolute transform for link
|
||||
// compute target transform for ik_frame applying motion transform of twist
|
||||
// linear+angular are expressed w.r.t. model frame and thus we need left-multiplication
|
||||
linear = frame_pose.linear() * linear;
|
||||
angular = frame_pose.linear() * angular;
|
||||
target_eigen = ik_pose_world;
|
||||
target_eigen.linear() =
|
||||
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular);
|
||||
target_eigen.translation() += linear;
|
||||
auto R = Eigen::AngleAxisd(angular_norm, angular);
|
||||
auto p = ik_pose_world.translation();
|
||||
target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
|
||||
goto COMPUTE;
|
||||
} catch (const boost::bad_any_cast&) { /* continue with Vector */
|
||||
}
|
||||
@ -267,31 +265,31 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
if (dir == Interface::BACKWARD)
|
||||
linear *= -1.0;
|
||||
|
||||
// compute absolute transform for link
|
||||
// compute target transform for ik_frame applying delta transform of twist
|
||||
linear = frame_pose.linear() * linear;
|
||||
target_eigen = ik_pose_world;
|
||||
target_eigen.translation() += linear;
|
||||
target_eigen = Eigen::Translation3d(linear) * ik_pose_world;
|
||||
} catch (const boost::bad_any_cast&) {
|
||||
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
|
||||
return false;
|
||||
}
|
||||
|
||||
COMPUTE:
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
|
||||
// offset from link to ik_frame
|
||||
const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||
|
||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success =
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
|
||||
double distance = 0.0;
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
@ -309,8 +307,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
// visualize plan
|
||||
auto ns = props.get<std::string>("marker_ns");
|
||||
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear,
|
||||
distance);
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
|
||||
linear, distance);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -228,11 +228,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
add_frame(target, "target frame");
|
||||
add_frame(ik_pose_world, "ik frame");
|
||||
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
|
||||
// offset from link to ik_frame
|
||||
Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||
|
||||
// plan to Cartesian target
|
||||
success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
}
|
||||
|
||||
// store result
|
||||
|
||||
@ -69,23 +69,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
} else {
|
||||
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(property.value());
|
||||
if (ik_pose_msg.header.frame_id.empty()) {
|
||||
if (!(robot_link = get_tip())) {
|
||||
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
|
||||
return false;
|
||||
}
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
|
||||
} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
|
||||
robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
|
||||
} else {
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
|
||||
robot_link = nullptr;
|
||||
bool found = false;
|
||||
auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found);
|
||||
if (!found && !ik_pose_msg.header.frame_id.empty()) {
|
||||
std::stringstream ss;
|
||||
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
|
||||
ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'";
|
||||
solution.markAsFailure(ss.str());
|
||||
return false;
|
||||
}
|
||||
if (!robot_link)
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
solution.markAsFailure("ik_frame doesn't specify a link frame");
|
||||
return false;
|
||||
} else if (!found) { // use robot link's frame as reference by default
|
||||
ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
}
|
||||
|
||||
tip_in_global_frame = ref_frame * tip_in_global_frame;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
@ -17,7 +18,7 @@ using namespace planning_scene;
|
||||
using namespace moveit::core;
|
||||
|
||||
constexpr double TAU{ 2 * M_PI };
|
||||
constexpr double EPS{ 1e-6 };
|
||||
constexpr double EPS{ 5e-5 };
|
||||
|
||||
// provide a basic test fixture that prepares a Task
|
||||
struct PandaMoveRelative : public testing::Test
|
||||
@ -66,10 +67,24 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
|
||||
return aco;
|
||||
}
|
||||
|
||||
inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) {
|
||||
return scene->getFrameTransform(frame).translation();
|
||||
void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip,
|
||||
const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) {
|
||||
const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast<const SubTrajectory>(solution)->trajectory();
|
||||
const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation();
|
||||
for (size_t i = 0; i < t.getWayPointCount(); ++i) {
|
||||
const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation();
|
||||
ASSERT_TRUE(start_position.isApprox(position, EPS))
|
||||
<< "Rotation must maintain position\n"
|
||||
<< i << ": " << start_position.transpose() << " != " << position.transpose();
|
||||
}
|
||||
}
|
||||
|
||||
#define EXPECT_CONST_POSITION(...) \
|
||||
{ \
|
||||
SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \
|
||||
expect_const_position(__VA_ARGS__); \
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianRotateEEF) {
|
||||
move->setDirection([] {
|
||||
geometry_msgs::TwistStamped twist;
|
||||
@ -79,15 +94,22 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) {
|
||||
}());
|
||||
|
||||
ASSERT_TRUE(t.plan()) << "Failed to plan";
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName());
|
||||
}
|
||||
|
||||
const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() };
|
||||
const auto start_eef_position{ position(scene, tip_name) };
|
||||
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) };
|
||||
TEST_F(PandaMoveRelative, cartesianCircular) {
|
||||
const std::string tip = "panda_hand";
|
||||
auto offset = Eigen::Translation3d(0, 0, 0.1);
|
||||
move->setIKFrame(offset, tip);
|
||||
move->setDirection([] {
|
||||
geometry_msgs::TwistStamped twist;
|
||||
twist.header.frame_id = "world";
|
||||
twist.twist.angular.x = TAU / 4.0;
|
||||
return twist;
|
||||
}());
|
||||
|
||||
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
|
||||
<< "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n"
|
||||
<< start_eef_position << "\nvs\n"
|
||||
<< end_eef_position;
|
||||
ASSERT_TRUE(t.plan()) << "Failed to plan";
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset));
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
|
||||
@ -102,15 +124,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
|
||||
return twist;
|
||||
}());
|
||||
|
||||
ASSERT_TRUE(t.plan());
|
||||
|
||||
const auto start_eef_position{ position(scene, ATTACHED_OBJECT) };
|
||||
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) };
|
||||
|
||||
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
|
||||
<< "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n"
|
||||
<< start_eef_position << "\nvs\n"
|
||||
<< end_eef_position;
|
||||
ASSERT_TRUE(t.plan()) << "Failed to plan";
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), ATTACHED_OBJECT);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user