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));
}
}
}
}
void HeatTransferBCHandling::init_bc_types( const BoundaryID bc_id,
const std::string& bc_id_string,
const int bc_type,
const std::string& bc_vars,
const std::string& bc_value,
const GetPot& input )
{
switch(bc_type)
{
case(ISOTHERMAL_WALL):
{
this->set_dirichlet_bc_type( bc_id, bc_type );
this->set_dirichlet_bc_value( bc_id, input("Physics/"+_physics_name+"/T_wall_"+bc_id_string, 0.0 ) );
}
break;
case(ADIABATIC_WALL):
{
this->set_neumann_bc_type( bc_id, bc_type );
}
break;
case(PRESCRIBED_HEAT_FLUX):
{
this->set_neumann_bc_type( bc_id, bc_type );
libMesh::RealGradient q_in;
int num_q_components = input.vector_variable_size("Physics/"+_physics_name+"/q_wall_"+bc_id_string);
for( int i = 0; i < num_q_components; i++ )
{
q_in(i) = input("Physics/"+_physics_name+"/q_wall_"+bc_id_string, 0.0, i );
}
this->set_neumann_bc_value( bc_id, q_in );
}
break;
case(GENERAL_HEAT_FLUX):
{
this->set_neumann_bc_type( bc_id, bc_type );
}
break;
default:
{
// Call base class to detect any physics-common boundary conditions
BCHandlingBase::init_bc_types( bc_id, bc_id_string, bc_type,
bc_vars, bc_value, input );
}
}// End switch(bc_type)
return;
}
bool TreeKinematics::getPositionFk(kinematics_msgs::GetPositionFK::Request& request,
kinematics_msgs::GetPositionFK::Response& response)
{
ROS_DEBUG("getPositionFK method invoked.");
KDL::JntArray q_in;
double nr_of_jnts = request.robot_state.joint_state.name.size();
q_in.resize(nr_of_jnts);
for (unsigned int i=0; i < nr_of_jnts; ++i)
{
int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
if (tmp_index >=0)
{
q_in(tmp_index) = request.robot_state.joint_state.position[i];
ROS_DEBUG("joint '%s' is now number '%d'", request.robot_state.joint_state.name[i].c_str(), tmp_index);
}
else
{
ROS_FATAL("i: %d, No joint index for %s!", i, request.robot_state.joint_state.name[i].c_str());
ROS_FATAL("Service call aborted.");
return false;
}
}
response.pose_stamped.resize(request.fk_link_names.size());
response.fk_link_names.resize(request.fk_link_names.size());
KDL::Frame p_out;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
for (unsigned int i=0; i < request.fk_link_names.size(); i++)
{
ROS_DEBUG("End effector name: %s",request.fk_link_names[i].c_str());
int fk_ret = fk_solver_->JntToCart(q_in, p_out, request.fk_link_names[i]);
if (fk_ret >= 0)
{
tf_pose.frame_id_ = tree_root_name_;
tf_pose.stamp_ = ros::Time::now();
tf::PoseKDLToTF(p_out, tf_pose);
try
{
tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
}
catch (tf::TransformException const &ex)
{
ROS_FATAL("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose, pose);
response.pose_stamped[i] = pose;
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
}
else
{
ROS_WARN("A FK solution could not be found for %s (error code = %d)",
request.fk_link_names[i].c_str(), fk_ret);
response.error_code.val = response.error_code.NO_FK_SOLUTION;
}
}
return true;
}
