void testJacobian(std::string group_name, std::string base_link, std::string tip_link)
{
SCOPED_TRACE(group_name + ": " + base_link + " to " + tip_link);
srand ( time(NULL) ); // initialize random seed:
rdf_loader::RDFLoader model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model;
kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));
robot_state::RobotStatePtr kinematic_state;
kinematic_state.reset(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_state_group = kinematic_state->getRobotModel()->getJointModelGroup(group_name);
std::string link_name = tip_link;
std::vector<double> joint_angles(7,0.0);
geometry_msgs::Point ref_position;
Eigen::MatrixXd jacobian;
Eigen::Vector3d point(0.0,0.0,0.0);
kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
ASSERT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name),point,jacobian));
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
{
ROS_ERROR("Could not initialize tree object");
}
KDL::Chain kdl_chain;
std::string base_frame(base_link);
std::string tip_frame(tip_link);
if (!tree.getChain(base_frame, tip_frame, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
}
KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
KDL::Jacobian jacobian_kdl(7);
KDL::JntArray q_in(7);
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
unsigned int NUM_TESTS = 10000;
for(unsigned int i=0; i < NUM_TESTS; i++)
{
for(int j=0; j < 7; j++)
{
q_in(j) = gen_rand(-M_PI,M_PI);
joint_angles[j] = q_in(j);
}
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
EXPECT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name), point, jacobian));
for(unsigned int k=0; k < 6; k++)
{
for(unsigned int m=0; m < 7; m++)
{
EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
}
}
}
}
TEST(JacobianSolver, solver)
{
srand ( time(NULL) ); // initialize random seed:
rdf_loader::RDFLoader model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model;
kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));
robot_state::RobotStatePtr kinematic_state;
kinematic_state.reset(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm");
std::string link_name = "r_wrist_roll_link";
std::vector<double> joint_angles(7,0.0);
geometry_msgs::Point ref_position;
Eigen::MatrixXd jacobian;
Eigen::Vector3d point(0.0,0.0,0.0);
joint_state_group->setVariableValues(joint_angles);
ASSERT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
{
ROS_ERROR("Could not initialize tree object");
}
KDL::Chain kdl_chain;
std::string base_frame("torso_lift_link");
std::string tip_frame("r_wrist_roll_link");
if (!tree.getChain(base_frame, tip_frame, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
}
KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
KDL::Jacobian jacobian_kdl(7);
KDL::JntArray q_in(7);
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
unsigned int NUM_TESTS = 1000000;
for(unsigned int i=0; i < NUM_TESTS; i++)
{
for(int j=0; j < 7; j++)
{
q_in(j) = gen_rand(-M_PI,M_PI);
joint_angles[j] = q_in(j);
}
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
joint_state_group->setVariableValues(joint_angles);
EXPECT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
for(unsigned int k=0; k < 6; k++)
{
for(unsigned int m=0; m < 7; m++)
{
EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
}
}
}
}
/**
* Show one tip-of-the-day in a frame on the titlescreen.
* This frame has 2 buttons: Next-Tip, and Show-Help.
*/
static void draw_tip_of_day(CVideo& screen,
config& tips_of_day,
const gui::dialog_frame::style& style,
const SDL_Rect* const main_dialog_area
)
{
if(preferences::show_tip_of_day() == false) {
return;
}
// Restore the previous tip of day area to its old state (section of the title image).
//tip_of_day_restorer.restore();
// Draw tip of the day
const config* tip = get_tip_of_day(tips_of_day);
if(tip != NULL) {
int tip_width = main_dialog_area->w; //game_config::title_tip_width * screen.w() / 1024;
try {
const std::string& text =
font::word_wrap_text((*tip)["text"], font::SIZE_NORMAL, tip_width);
const std::string& source =
font::word_wrap_text((*tip)["source"], font::SIZE_NORMAL, tip_width);
// const int pad = game_config::title_tip_padding;
#ifdef __IPAD__
const int pad = 10;
#else
const int pad = 5;
#endif
SDL_Rect area = font::text_area(text,font::SIZE_NORMAL);
area.w = tip_width;
SDL_Rect source_area = font::text_area(source, font::SIZE_NORMAL, TTF_STYLE_ITALIC);
area.w = std::max<size_t>(area.w, source_area.w) + 2*pad;
//area.x = main_dialog_area->x; // - (game_config::title_tip_x * screen.w() / 1024) - area.w;
//area.y = main_dialog_area->y + (main_dialog_area->h - area.h)/2;
SDL_Rect total_area;
total_area.w = area.w;
total_area.h = area.h + source_area.h + 3*pad;
total_area.x = main_dialog_area->x;
total_area.y = main_dialog_area->y + (main_dialog_area->h - total_area.h)/2;
source_area.y = total_area.y + area.h + pad;
// Note: The buttons' locations need to be set before the dialog frame is drawn.
// Otherwise, when the buttons restore their area, they
// draw parts of the old dialog frame at their old locations.
// This way, the buttons draw a part of the title image,
// because the call to restore above restored the area
// of the old tip of the day to its initial state (the title image).
// int button_x = area.x + area.w - next_tip_button->location().w - pad;
// int button_y = area.y + area.h - pad - next_tip_button->location().h;
// next_tip_button->set_location(button_x, button_y);
// next_tip_button->set_dirty(); //force redraw even if location did not change.
// button_x -= previous_tip_button->location().w + pad;
// previous_tip_button->set_location(button_x, button_y);
// previous_tip_button->set_dirty();
// button_x = area.x + pad;
// if (help_tip_button != NULL)
// {
// help_tip_button->set_location(button_x, button_y);
// help_tip_button->set_dirty();
// }
gui::dialog_frame tip_frame(screen, "", gui::dialog_frame::titlescreen_style, false);
SDL_Rect borderRect = total_area;
tip_frame.layout(borderRect);
tip_frame.draw_background();
tip_frame.draw_border();
// gui::dialog_frame f(screen, "", style, false);
// tip_of_day_restorer = surface_restorer(&screen.video(), f.layout(area).exterior);
// f.draw_background();
// f.draw_border();
font::draw_text(&screen, total_area, font::SIZE_NORMAL, font::NORMAL_COLOUR,
text, total_area.x + pad, total_area.y + pad);
font::draw_text(&screen, total_area, font::SIZE_NORMAL, font::NORMAL_COLOUR,
source, total_area.x + total_area.w - source_area.w - pad,
source_area.y,
false, TTF_STYLE_ITALIC);
} catch (utils::invalid_utf8_exception&) {
LOG_STREAM(err, engine) << "Invalid utf-8 found, tips of day aren't drawn.\n";
return;
}
// LOG_DP << "drew tip of day\n";
}
}
