TEST(Matrix, Multiplication)
{
Vector3 v(1.0f, 1.0f, 1.0f);
float trans[4][4] = { 3.0f, 0.0f, 0.0f, 0.0f,
0.0f, 3.0f, 0.0f, 0.0f,
0.0f, 0.0f, 3.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
Matrix4 world_transform(trans);
float trans2[4][4] = { 1.0f, 0.0f, 0.0f, 5.0f,
0.0f, 1.0f, 0.0f, 10.0f,
0.0f, 0.0f, 1.0f, -4.0f,
0.0f, 0.0f, 0.0f, 1.0f };
Matrix4 translate(trans2);
//world_transform.Multiply(translate);
world_transform *= translate;
v.Transform(world_transform);
EXPECT_NEAR(18.0f, v.GetX(), 0.01f);
EXPECT_NEAR(33.0f, v.GetY(), 0.01f);
EXPECT_NEAR(-9.0f, v.GetZ(), 0.01f);
}
bool transform(state_transformer::GetTransform::Request &req,
state_transformer::GetTransform::Response &res) {
planning_models::KinematicState kstate(robot_model.getKinematicModel());
if (!planning_environment::setRobotStateAndComputeTransforms
(req.robot_state, kstate)) {
ROS_ERROR("Unable to transform robot state to kinematic state");
res.val = res.BAD_ROBOT_STATE;
return true;
}
std::string from_frame = relative_frame(req.from_frame_id);
std::string to_frame = relative_frame(req.to_frame_id);
ROS_INFO("Transforming from %s to %s", from_frame.c_str(), to_frame.c_str());
std::vector<geometry_msgs::TransformStamped> transforms;
btTransform global_to_to, global_to_from;
if (!world_transform(from_frame, kstate, global_to_from)) {
res.val = res.UNABLE_TO_FIND_FROM_FRAME;
return true;
}
if (!world_transform(to_frame, kstate, global_to_to)) {
res.val = res.UNABLE_TO_FIND_TO_FRAME;
return true;
}
//This is the transform that will take the origin of to
//to the origin of from. This is how TF works so we are sticking
//with that convention although it is confusing. The transform
//with from=robot frame to=wrist frame will give you the position of
//the wrist in the robot frame for example.
//
//HOWEVER, the transform that takes a pose expressed in from and transforms
//it to a pose expressed in to is the INVERSE of this transform
btTransform trans = (global_to_from.inverse())*global_to_to;
tf::transformTFToMsg(trans, res.transform_stamped.transform);
res.transform_stamped.header.stamp = ros::Time(0);
res.transform_stamped.header.frame_id = req.from_frame_id;
res.transform_stamped.child_frame_id = req.to_frame_id;
res.val = res.SUCCESS;
return true;
}
Ray const RayNode::get_world_ray() const {
math::mat4 world_transform(get_world_transform());
math::vec4 origin(0, 0, 0, 1.0);
math::vec4 direction(0, 0, -1, 0.0);
origin = world_transform * origin;
direction = world_transform * direction;
return Ray(origin, direction, 1.0);
}
TEST(Matrix, Subtraction)
{
Vector3 v(1.0f, 1.0f, 1.0f);
float trans[4][4] = { 3.0f, 0.0f, 0.0f, 0.0f,
0.0f, 3.0f, 0.0f, 0.0f,
0.0f, 0.0f, 3.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
Matrix4 world_transform(trans);
float trans2[4][4] = { 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
Matrix4 translate(trans2);
world_transform.Sub(translate);
v.Transform(world_transform);
EXPECT_NEAR(2.0f, v.GetX(), 0.01f);
EXPECT_NEAR(2.0f, v.GetY(), 0.01f);
EXPECT_NEAR(2.0f, v.GetZ(), 0.01f);
}
