Skip to content

Commit

Permalink
randomWalkIK
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke @v4hn
rhaschke authored and v4hn committed Dec 21, 2018
1 parent 62942dd commit f0685fb
Show file tree
Hide file tree
Showing 3 changed files with 89 additions and 16 deletions.
4 changes: 2 additions & 2 deletions moveit_kinematics/test/fanuc.test
View file
Expand Up @@ -8,8 +8,8 @@
<param name="tip_link" value="tool0"/>
<param name="root_link" value="base_link"/>
<param name="group" value="manipulator"/>
<param name="num_fk_tests" value="20"/>
<param name="num_ik_tests" value="20"/>
<param name="num_fk_tests" value="100"/>
<param name="num_ik_tests" value="100"/>
<param name="num_ik_cb_tests" value="20"/>
<param name="num_ik_multiple_tests" value="20"/>
<param name="num_nearest_ik_tests" value="0"/>
Expand Down
4 changes: 2 additions & 2 deletions moveit_kinematics/test/panda.test
View file
Expand Up @@ -10,8 +10,8 @@
<param name="group" value="panda_arm"/>
<rosparam param="joint_names">[panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7]</rosparam>

<param name="num_fk_tests" value="20"/>
<param name="num_ik_tests" value="20"/>
<param name="num_fk_tests" value="100"/>
<param name="num_ik_tests" value="100"/>
<param name="num_ik_cb_tests" value="20"/>
<param name="num_ik_multiple_tests" value="20"/>
<param name="num_nearest_ik_tests" value="0"/>
Expand Down
97 changes: 85 additions & 12 deletions moveit_kinematics/test/test_kinematics_plugin.cpp
View file
Expand Up @@ -48,6 +48,10 @@
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit/robot_trajectory/robot_trajectory.h>

const double IK_NEAR = 1e-5;

const std::string ROBOT_DESCRIPTION_PARAM = "robot_description";
Expand Down Expand Up @@ -172,7 +176,8 @@ class KinematicsTest : public ::testing::Test
ASSERT_TRUE(kinematics_solver_->initialize(*robot_model_, group_name_, root_link_, { tip_link_ },
DEFAULT_SEARCH_DISCRETIZATION) ||
kinematics_solver_->initialize(ROBOT_DESCRIPTION_PARAM, group_name_, root_link_, { tip_link_ },
DEFAULT_SEARCH_DISCRETIZATION)) << "Solver failed to initialize";
DEFAULT_SEARCH_DISCRETIZATION))
<< "Solver failed to initialize";

jmg_ = robot_model_->getJointModelGroup(kinematics_solver_->getGroupName());
ASSERT_TRUE(jmg_);
Expand All @@ -185,19 +190,19 @@ class KinematicsTest : public ::testing::Test
}

public:
void expectNear(const std::vector<geometry_msgs::Pose>& first,
const std::vector<geometry_msgs::Pose>& second)
void expectNear(const std::vector<geometry_msgs::Pose>& first, const std::vector<geometry_msgs::Pose>& second,
double near = IK_NEAR)
{
EXPECT_EQ(first.size(), second.size());
for (size_t i=0; i < first.size(); ++i)
for (size_t i = 0; i < first.size(); ++i)
{
EXPECT_NEAR(first[i].position.x, second[i].position.x, IK_NEAR);
EXPECT_NEAR(first[i].position.y, second[i].position.y, IK_NEAR);
EXPECT_NEAR(first[i].position.z, second[i].position.z, IK_NEAR);
EXPECT_NEAR(first[i].orientation.x, second[i].orientation.x, IK_NEAR);
EXPECT_NEAR(first[i].orientation.y, second[i].orientation.y, IK_NEAR);
EXPECT_NEAR(first[i].orientation.z, second[i].orientation.z, IK_NEAR);
EXPECT_NEAR(first[i].orientation.w, second[i].orientation.w, IK_NEAR);
EXPECT_NEAR(first[i].position.x, second[i].position.x, near);
EXPECT_NEAR(first[i].position.y, second[i].position.y, near);
EXPECT_NEAR(first[i].position.z, second[i].position.z, near);
EXPECT_NEAR(first[i].orientation.x, second[i].orientation.x, near);
EXPECT_NEAR(first[i].orientation.y, second[i].orientation.y, near);
EXPECT_NEAR(first[i].orientation.z, second[i].orientation.z, near);
EXPECT_NEAR(first[i].orientation.w, second[i].orientation.w, near);
}
}

Expand Down Expand Up @@ -261,6 +266,73 @@ TEST_F(KinematicsTest, getFK)
}
}

#define PUBLISH_TRAJECTORY 0
// perform random walk in joint-space, reaching poses via IK
TEST_F(KinematicsTest, randomWalkIK)
{
std::vector<double> seed, goal, solution;
const std::vector<std::string>& tip_frames = kinematics_solver_->getTipFrames();
robot_state::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();

#if PUBLISH_TRAJECTORY
moveit_msgs::DisplayTrajectory msg;
msg.model_id = robot_model_->getName();
moveit::core::robotStateToRobotStateMsg(robot_state, msg.trajectory_start);
msg.trajectory.resize(1);
robot_trajectory::RobotTrajectory traj(robot_model_, jmg_);
#endif

unsigned int successes = 0;
constexpr double NEAR_JOINT = 0.1;
for (unsigned int i = 0; i < num_ik_tests_; ++i)
{
// remember previous pose
robot_state.copyJointGroupPositions(jmg_, seed);
// sample a new pose nearby
robot_state.setToRandomPositionsNearBy(jmg_, robot_state, NEAR_JOINT);
// get joints of new pose
robot_state.copyJointGroupPositions(jmg_, goal);
// compute target tip_frames
std::vector<geometry_msgs::Pose> poses;
ASSERT_TRUE(kinematics_solver_->getPositionFK(tip_frames, goal, poses));

// compute IK
moveit_msgs::MoveItErrorCodes error_code;
kinematics_solver_->getPositionIK(poses[0], seed, solution, error_code);
EXPECT_EQ(error_code.val, error_code.SUCCESS);

// validate closeness of solution pose to goal
auto diff = Eigen::Map<Eigen::ArrayXd>(solution.data(), solution.size()) -
Eigen::Map<Eigen::ArrayXd>(goal.data(), goal.size());
if (diff.isZero(0.1))
++successes;
else
ROS_WARN_STREAM("jump in [" << i << "]: " << diff.transpose());

// validate reached poses
std::vector<geometry_msgs::Pose> reached_poses;
kinematics_solver_->getPositionFK(tip_frames, solution, reached_poses);
expectNear(poses, reached_poses);

// update robot state to found pose
robot_state.setJointGroupPositions(jmg_, solution);
#if PUBLISH_TRAJECTORY
traj.addSuffixWayPoint(robot_state, 0.1);
#endif
}
ROS_INFO_STREAM("Successes: " << successes << " / " << num_ik_tests_);
// EXPECT_GT(successes, EXPECTED_SUCCESS_RATE * num_ik_tests_);

#if PUBLISH_TRAJECTORY
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<moveit_msgs::DisplayTrajectory>("display_random_walk", 1, true);
traj.getRobotTrajectoryMsg(msg.trajectory[0]);
pub.publish(msg);
ros::spin();
#endif
}

TEST_F(KinematicsTest, searchIK)
{
std::vector<double> seed, fk_values, solution;
Expand Down Expand Up @@ -317,7 +389,7 @@ TEST_F(KinematicsTest, searchIKWithCallback)
continue;

kinematics_solver_->searchPositionIK(poses[0], fk_values, timeout, solution,
boost::bind(&KinematicsTest::searchIKCallback, this, _1, _2, _3), error_code);
boost::bind(&KinematicsTest::searchIKCallback, this, _1, _2, _3), error_code);
if (error_code.val == error_code.SUCCESS)
success++;
else
Expand Down Expand Up @@ -508,6 +580,7 @@ int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "kinematics_plugin_test");
ros::NodeHandle nh;
int result = RUN_ALL_TESTS();
SharedData::release(); // avoid class_loader::LibraryUnloadException
return result;
Expand Down

0 comments on commit f0685fb

Please sign in to comment.