TEST(test_iter, iterate)
{
KeySet ks2 (5,
*Key ("user/key2/1", KEY_END),
*Key ("user/key2/2", KEY_END),
*Key ("user/key2/3", KEY_VALUE, "value", KEY_END),
KS_END);
ASSERT_EQ((*ks2.begin()).getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ(ks2.begin()->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((ks2.begin()[0])->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((*(++ks2.begin())).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((++ks2.begin())->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.begin()+1)).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((ks2.begin()+1)->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((ks2.begin()[1])->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.begin()+2)).getName() , "user/key2/3") << "name wrong";
ASSERT_EQ((ks2.begin()+2)->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((ks2.begin()[2])->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((ks2.begin()+3) , ks2.end() ) << "end iterator broken";
ASSERT_EQ((*ks2.rbegin()).getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ(ks2.rbegin()->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((ks2.rbegin()[0])->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((*(++ks2.rbegin())).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((++ks2.rbegin())->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.rbegin()+1)).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((ks2.rbegin()+1)->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((ks2.rbegin()[1])->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.rbegin()+2)).getName() , "user/key2/1") << "name wrong";
ASSERT_EQ((ks2.rbegin()+2)->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((ks2.rbegin()[2])->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((ks2.rbegin()+3) , ks2.rend() ) << "end iterator broken";
ASSERT_EQ((*ks2.cbegin()).getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ(ks2.cbegin()->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((ks2.cbegin()[0])->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((*(++ks2.cbegin())).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((++ks2.cbegin())->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.cbegin()+1)).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((ks2.cbegin()+1)->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((ks2.cbegin()[1])->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.cbegin()+2)).getName() , "user/key2/3") << "name wrong";
ASSERT_EQ((ks2.cbegin()+2)->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((ks2.cbegin()[2])->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((ks2.cbegin()+3) , ks2.cend() ) << "end iterator broken";
ASSERT_EQ((*ks2.crbegin()).getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ(ks2.crbegin()->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((ks2.crbegin()[0])->getName() , "user/key2/3" ) << "name wrong";
ASSERT_EQ((*(++ks2.crbegin())).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((++ks2.crbegin())->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.crbegin()+1)).getName() , "user/key2/2") << "name wrong";
ASSERT_EQ((ks2.crbegin()+1)->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((ks2.crbegin()[1])->getName() , "user/key2/2" ) << "name wrong";
ASSERT_EQ((*(ks2.crbegin()+2)).getName() , "user/key2/1") << "name wrong";
ASSERT_EQ((ks2.crbegin()+2)->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((ks2.crbegin()[2])->getName() , "user/key2/1" ) << "name wrong";
ASSERT_EQ((ks2.crbegin()+3) , ks2.crend() ) << "end iterator broken";
}
void TripleDESCryptor::Decrypt(const char* input, char* output, size_t length) const
{
int n = 0;
DES_cblock ivec = {0};
DES_key_schedule ks1(ks1_);
DES_key_schedule ks2(ks2_);
DES_key_schedule ks3(ks3_);
DES_ede3_cfb64_encrypt((const unsigned char*)input, (unsigned char*)output, length, &ks1, &ks2, &ks3, &ivec, &n, DES_DECRYPT);
}
void check (kdb::KeySet &ks) override
{
kdb::Key k;
kdb::KeySet ks2 (ks.dup());
ks2.rewind();
kdb::Key root = ks2.next();
if (!root) throw "ListChecker: no root key found";
while ((k = ks2.next()))
{
if (!root.isDirectBelow(k)) throw "ListChecker: key is not direct below";
kdb::KeySet cks(ks2.cut(k));
structure->check(cks);
}
}
TEST_F(LoadPlanningModelsPr2, FullTest)
{
moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
moveit::core::RobotState ks(robot_model);
ks.setToDefaultValues();
moveit::core::RobotState ks2(robot_model);
ks2.setToDefaultValues();
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Isometry3d poses;
shapes::Shape* shape = new shapes::Box(.1, .1, .1);
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Isometry3d::Identity());
std::set<std::string> touch_links;
trajectory_msgs::JointTrajectory empty_state;
moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody(
robot_model->getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
ks.attachBody(attached_body);
std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(), 1u);
std::vector<const moveit::core::AttachedBody*> attached_bodies_2;
ks2 = ks;
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(), 1u);
ks.clearAttachedBody("box");
attached_bodies_1.clear();
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(), 0u);
ks2 = ks;
attached_bodies_2.clear();
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(), 0u);
}
//TEST_F(LoadPlanningModelsPr2, robot_state::RobotState *Copy)
TEST_F(LoadPlanningModelsPr2, FullTest)
{
robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
robot_state::RobotState ks2(kmodel);
ks2.setToDefaultValues();
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes::Shape* shape = new shapes::Box(.1,.1,.1);
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Affine3d::Identity());
std::set<std::string> touch_links;
robot_state::AttachedBody attached_body(ks.getLinkState("r_gripper_palm_link")->getLinkModel(), "box", shapes, poses, touch_links);
ks.attachBody(&attached_body);
std::vector<const robot_state::AttachedBody*> attached_bodies_1;
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(),1);
std::vector<const robot_state::AttachedBody*> attached_bodies_2;
ks2 = ks;
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(),1);
ks.clearAttachedBody("box");
attached_bodies_1.clear();
ks.getAttachedBodies(attached_bodies_1);
ASSERT_EQ(attached_bodies_1.size(),0);
ks2 = ks;
attached_bodies_2.clear();
ks2.getAttachedBodies(attached_bodies_2);
ASSERT_EQ(attached_bodies_2.size(),0);
}
vector<CubicBezierControlPoints> RRTPlanner::generateCubicBezierPath(
const vector<Geometry2d::Point>& points,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, const boost::optional<vector<float>>& times) {
size_t length = points.size();
size_t curvesNum = length - 1;
vector<double> pointsX(length);
vector<double> pointsY(length);
vector<double> ks(length - 1);
vector<double> ks2(length - 1);
for (int i = 0; i < length; i++) {
pointsX[i] = points[i].x;
pointsY[i] = points[i].y;
}
const float startSpeed = vi.mag();
const float endSpeed = vf.mag();
if (times) {
assert(times->size() == points.size());
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (times->at(i + 1) - times->at(i));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
} else {
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (getTime(points, i + 1, motionConstraints, startSpeed,
endSpeed) -
getTime(points, i, motionConstraints, startSpeed,
endSpeed));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
}
VectorXd solutionX =
RRTPlanner::cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
VectorXd solutionY =
RRTPlanner::cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);
vector<CubicBezierControlPoints> path;
for (int i = 0; i < curvesNum; i++) {
Point p0 = points[i];
Point p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
Point p2 =
Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
Point p3 = points[i + 1];
path.emplace_back(p0, p1, p2, p3);
}
return path;
}
// TODO: Use targeted end velocity
InterpolatedPath* RRTPlanner::cubicBezier(
InterpolatedPath& path, const Geometry2d::ShapeSet* obstacles,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf) {
int length = path.waypoints.size();
int curvesNum = length - 1;
if (curvesNum <= 0) {
delete &path;
return nullptr;
}
// TODO: Get the actual values
vector<double> pointsX(length);
vector<double> pointsY(length);
vector<double> ks(length - 1);
vector<double> ks2(length - 1);
for (int i = 0; i < length; i++) {
pointsX[i] = path.waypoints[i].pos().x;
pointsY[i] = path.waypoints[i].pos().y;
}
float startSpeed = vi.mag();
// This is pretty hacky;
Geometry2d::Point startDirection =
(path.waypoints[1].pos() - path.waypoints[0].pos()).normalized();
if (startSpeed < 0.3) {
startSpeed = 0.3;
vi = startDirection * startSpeed;
} else {
vi = vi.mag() * (startDirection + vi.normalized()) / 2.0 * 0.8;
}
const float endSpeed = vf.mag();
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 /
(getTime(path, i + 1, motionConstraints, startSpeed, endSpeed) -
getTime(path, i, motionConstraints, startSpeed, endSpeed));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
delete &path;
return nullptr;
}
}
VectorXd solutionX = cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
VectorXd solutionY = cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);
Geometry2d::Point p0, p1, p2, p3;
vector<InterpolatedPath::Entry> pts;
const int interpolations = 10;
double time = 0;
for (int i = 0; i < curvesNum; i++) {
p0 = path.waypoints[i].pos();
p3 = path.waypoints[i + 1].pos();
p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
p2 = Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
for (int j = 0; j < interpolations; j++) {
double k = ks[i];
float t = (((float)j / (float)(interpolations)));
Geometry2d::Point pos =
pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 +
3 * (1.0 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;
t = ((float)j / (float)(interpolations)) / k;
// 3 k (-(A (-1 + k t)^2) + k t (2 C - 3 C k t + D k t) + B (1 - 4 k
// t + 3 k^2 t^2))
Geometry2d::Point vel =
3 * k * (-(p0 * pow(-1 + k * t, 2)) +
k * t * (2 * p2 - 3 * p2 * k * t + p3 * k * t) +
p1 * (1 - 4 * k * t + 3 * pow(k, 2) * pow(t, 2)));
pts.emplace_back(MotionInstant(pos, vel), time + t);
}
time += 1.0 / ks[i];
}
pts.emplace_back(MotionInstant(path.waypoints[length - 1].pos(), vf), time);
path.waypoints = pts;
return &path;
}
TEST_F(LoadPlanningModelsPr2, GroupInit)
{
static const std::string SMODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"pr2_test\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>"
"<group name=\"left_arm_base_tip\">"
"<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>"
"</group>"
"<group name=\"left_arm_joints\">"
"<joint name=\"l_monkey_pan_joint\"/>"
"<joint name=\"l_monkey_fles_joint\"/>"
"</group>"
"</robot>";
boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
srdfModel->initString(*urdf_model_, SMODEL1);
planning_models::KinematicModel kin_model1(urdf_model_, srdfModel);
const planning_models::KinematicModel::JointModelGroup* left_arm_base_tip_group = kin_model1.getJointModelGroup("left_arm_base_tip");
ASSERT_TRUE(left_arm_base_tip_group == NULL);
const planning_models::KinematicModel::JointModelGroup* left_arm_joints_group = kin_model1.getJointModelGroup("left_arm_joints");
ASSERT_TRUE(left_arm_joints_group == NULL);
static const std::string SMODEL2 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"pr2_test\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>"
"<group name=\"left_arm_base_tip\">"
"<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>"
"</group>"
"<group name=\"left_arm_joints\">"
"<joint name=\"l_shoulder_pan_joint\"/>"
"<joint name=\"l_shoulder_lift_joint\"/>"
"<joint name=\"l_upper_arm_roll_joint\"/>"
"<joint name=\"l_elbow_flex_joint\"/>"
"<joint name=\"l_forearm_roll_joint\"/>"
"<joint name=\"l_wrist_flex_joint\"/>"
"<joint name=\"l_wrist_roll_joint\"/>"
"</group>"
"</robot>";
srdfModel->initString(*urdf_model_, SMODEL2);
planning_models::KinematicModelPtr kin_model2(new planning_models::KinematicModel(urdf_model_, srdfModel));
left_arm_base_tip_group = kin_model2->getJointModelGroup("left_arm_base_tip");
ASSERT_TRUE(left_arm_base_tip_group != NULL);
left_arm_joints_group = kin_model2->getJointModelGroup("left_arm_joints");
ASSERT_TRUE(left_arm_joints_group != NULL);
EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 7);
EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7);
EXPECT_EQ(left_arm_joints_group->getActiveDOFNames().size(), left_arm_joints_group->getVariableCount());
EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7);
EXPECT_EQ(kin_model2->getActiveDOFNames().size(), kin_model2->getVariableCount());
bool found_shoulder_pan_link = false;
bool found_wrist_roll_link = false;
for(unsigned int i = 0; i < left_arm_base_tip_group->getLinkModels().size(); i++)
{
if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_shoulder_pan_link")
{
EXPECT_TRUE(found_shoulder_pan_link == false);
found_shoulder_pan_link = true;
}
if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_wrist_roll_link")
{
EXPECT_TRUE(found_wrist_roll_link == false);
found_wrist_roll_link = true;
}
EXPECT_TRUE(left_arm_base_tip_group->getLinkModels()[i]->getName() != "torso_lift_link");
}
EXPECT_TRUE(found_shoulder_pan_link);
EXPECT_TRUE(found_wrist_roll_link);
planning_models::KinematicState ks(kin_model2);
ks.setToDefaultValues();
std::map<std::string, double> jv;
jv["base_joint.x"] = 0.433;
jv["base_joint.theta"] = -0.5;
ks.setStateValues(jv);
moveit_msgs::RobotState robot_state;
planning_models::kinematicStateToRobotState(ks, robot_state);
planning_models::KinematicState ks2(kin_model2);
robotStateToKinematicState(robot_state, ks2);
std::vector<double> v1;
ks.getStateValues(v1);
std::vector<double> v2;
ks2.getStateValues(v2);
EXPECT_TRUE(v1.size() == v2.size());
for (unsigned int i = 0; i < v1.size(); ++i)
EXPECT_NEAR(v1[i], v2[i], 1e-5);
geometry_msgs::Quaternion q;
q.x = q.y = q.z = q.w = 0.0;
Eigen::Quaterniond tq;
EXPECT_FALSE(planning_models::quatFromMsg(q, tq));
EXPECT_TRUE(tq.w() == 1.0);
std::vector<double> state_double_vector;
ks.getStateValues(state_double_vector);
ASSERT_TRUE(ks.setStateValues(state_double_vector));
}
TEST_F(LoadPlanningModelsPr2, GroupInit)
{
static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
"<robot name=\"pr2\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
"parent_frame=\"base_footprint\" type=\"planar\"/>"
"<group name=\"left_arm_base_tip\">"
"<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>"
"</group>"
"<group name=\"left_arm_joints\">"
"<joint name=\"l_monkey_pan_joint\"/>"
"<joint name=\"l_monkey_fles_joint\"/>"
"</group>"
"</robot>";
srdf::ModelSharedPtr srdf_model(new srdf::Model());
srdf_model->initString(*urdf_model_, SMODEL1);
moveit::core::RobotModel robot_model1(urdf_model_, srdf_model);
const moveit::core::JointModelGroup* left_arm_base_tip_group = robot_model1.getJointModelGroup("left_arm_base_tip");
ASSERT_TRUE(left_arm_base_tip_group == nullptr);
const moveit::core::JointModelGroup* left_arm_joints_group = robot_model1.getJointModelGroup("left_arm_joints");
ASSERT_TRUE(left_arm_joints_group == nullptr);
static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
"<robot name=\"pr2\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
"parent_frame=\"base_footprint\" type=\"planar\"/>"
"<group name=\"left_arm_base_tip\">"
"<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>"
"</group>"
"<group name=\"left_arm_joints\">"
"<joint name=\"l_shoulder_pan_joint\"/>"
"<joint name=\"l_shoulder_lift_joint\"/>"
"<joint name=\"l_upper_arm_roll_joint\"/>"
"<joint name=\"l_elbow_flex_joint\"/>"
"<joint name=\"l_forearm_roll_joint\"/>"
"<joint name=\"l_wrist_flex_joint\"/>"
"<joint name=\"l_wrist_roll_joint\"/>"
"</group>"
"</robot>";
srdf_model->initString(*urdf_model_, SMODEL2);
moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model_, srdf_model));
left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip");
ASSERT_TRUE(left_arm_base_tip_group != nullptr);
left_arm_joints_group = robot_model2->getJointModelGroup("left_arm_joints");
ASSERT_TRUE(left_arm_joints_group != nullptr);
EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 9u);
EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7u);
EXPECT_EQ(left_arm_joints_group->getVariableNames().size(), left_arm_joints_group->getVariableCount());
EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7u);
EXPECT_EQ(robot_model2->getVariableNames().size(), robot_model2->getVariableCount());
bool found_shoulder_pan_link = false;
bool found_wrist_roll_link = false;
for (const moveit::core::LinkModel* link_model : left_arm_base_tip_group->getLinkModels())
{
if (link_model->getName() == "l_shoulder_pan_link")
{
EXPECT_TRUE(!found_shoulder_pan_link);
found_shoulder_pan_link = true;
}
if (link_model->getName() == "l_wrist_roll_link")
{
EXPECT_TRUE(!found_wrist_roll_link);
found_wrist_roll_link = true;
}
EXPECT_TRUE(link_model->getName() != "torso_lift_link");
}
EXPECT_TRUE(found_shoulder_pan_link);
EXPECT_TRUE(found_wrist_roll_link);
moveit::core::RobotState ks(robot_model2);
ks.setToDefaultValues();
std::map<std::string, double> jv;
jv["base_joint/x"] = 0.433;
jv["base_joint/theta"] = -0.5;
ks.setVariablePositions(jv);
moveit_msgs::RobotState robot_state;
moveit::core::robotStateToRobotStateMsg(ks, robot_state);
moveit::core::RobotState ks2(robot_model2);
moveit::core::robotStateMsgToRobotState(robot_state, ks2);
const double* v1 = ks.getVariablePositions();
const double* v2 = ks2.getVariablePositions();
for (std::size_t i = 0; i < ks.getVariableCount(); ++i)
EXPECT_NEAR(v1[i], v2[i], 1e-5);
}
