void
MomentumBuoyancyBoussinesqSrcElemKernel<AlgTraits>::execute(
SharedMemView<DoubleType**>& /* lhs */,
SharedMemView<DoubleType*>& rhs,
ScratchViews<DoubleType>& scratchViews)
{
SharedMemView<DoubleType*>& v_temperatureNp1 = scratchViews.get_scratch_view_1D(*temperatureNp1_);
SharedMemView<DoubleType*>& v_scv_volume = scratchViews.get_me_views(CURRENT_COORDINATES).scv_volume;
for (int ip=0; ip < AlgTraits::numScvIp_; ++ip) {
const int nearestNode = ipNodeMap_[ip];
DoubleType temperatureIp = 0.0;
for (int ic=0; ic < AlgTraits::nodesPerElement_; ++ic) {
const DoubleType r = v_shape_function_(ip, ic);
temperatureIp += r * v_temperatureNp1(ic);
}
// Compute RHS
const DoubleType scV = v_scv_volume(ip);
const int nnNdim = nearestNode * AlgTraits::nDim_;
const DoubleType fac = -rhoRef_ * beta_ * (temperatureIp - tRef_) * scV;
for (int j=0; j < AlgTraits::nDim_; ++j) {
rhs(nnNdim + j) += fac * gravity_(j);
}
// No LHS contributions
}
}
MomentumBuoyancyBoussinesqSrcElemKernel<AlgTraits>::MomentumBuoyancyBoussinesqSrcElemKernel(
const stk::mesh::BulkData& bulkData,
const SolutionOptions& solnOpts,
ElemDataRequests& dataPreReqs)
: Kernel(),
rhoRef_(solnOpts.referenceDensity_),
ipNodeMap_(sierra::nalu::MasterElementRepo::get_volume_master_element(AlgTraits::topo_)->ipNodeMap())
{
const stk::mesh::MetaData& metaData = bulkData.mesh_meta_data();
ScalarFieldType *temperature = metaData.get_field<ScalarFieldType>(stk::topology::NODE_RANK, "temperature");
temperatureNp1_ = &(temperature->field_of_state(stk::mesh::StateNP1));
coordinates_ = metaData.get_field<VectorFieldType>(stk::topology::NODE_RANK, solnOpts.get_coordinates_name());
const std::vector<double>& solnOptsGravity = solnOpts.get_gravity_vector(AlgTraits::nDim_);
for (int i = 0; i < AlgTraits::nDim_; i++)
gravity_(i) = solnOptsGravity[i];
tRef_ = solnOpts.referenceTemperature_;
rhoRef_ = solnOpts.referenceDensity_;
beta_ = solnOpts.thermalExpansionCoeff_;
MasterElement* meSCV = sierra::nalu::MasterElementRepo::get_volume_master_element(AlgTraits::topo_);
get_scv_shape_fn_data<AlgTraits>([&](double* ptr){meSCV->shape_fcn(ptr);}, v_shape_function_);
// add master elements
dataPreReqs.add_cvfem_volume_me(meSCV);
// fields and data
dataPreReqs.add_coordinates_field(*coordinates_, AlgTraits::nDim_, CURRENT_COORDINATES);
dataPreReqs.add_gathered_nodal_field(*temperatureNp1_, 1);
dataPreReqs.add_master_element_call(SCV_VOLUME, CURRENT_COORDINATES);
}
// Init KDL stuff
bool LWRHW::initKDLdescription(const urdf::Model *const urdf_model)
{
// KDL code to compute f_dyn(q)
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
{
ROS_ERROR("Failed to construct kdl tree");
return false;
}
std::cout << "LWR kinematic successfully parsed with "
<< kdl_tree.getNrOfJoints()
<< " joints, and "
<< kdl_tree.getNrOfJoints()
<< " segments." << std::endl;
// Get the info from parameters
std::string root_name;
ros::param::get(std::string("/") + robot_namespace_ + std::string("/root"), root_name);
if( root_name.empty() )
root_name = kdl_tree.getRootSegment()->first; // default
std::string tip_name;
ros::param::get(std::string("/") + robot_namespace_ + std::string("/tip"), tip_name);
if( tip_name.empty() )
tip_name = robot_namespace_ + std::string("_7_link"); ; // default
std::cout << "Using root: " << root_name << " and tip: " << tip_name << std::endl;
// this depends on how the world frame is set, in all our setups, world has always positive z pointing up.
gravity_ = KDL::Vector::Zero();
gravity_(2) = -9.81;
// Extract the chain from the tree
if(!kdl_tree.getChain(root_name, tip_name, lwr_chain_))
{
ROS_ERROR("Failed to get KDL chain from tree: ");
return false;
}
ROS_INFO("Number of segments: %d", lwr_chain_.getNrOfSegments());
ROS_INFO("Number of joints in chain: %d", lwr_chain_.getNrOfJoints());
f_dyn_solver_.reset(new KDL::ChainDynParam(lwr_chain_,gravity_));
joint_position_kdl_ = KDL::JntArray(lwr_chain_.getNrOfJoints());
gravity_effort_ = KDL::JntArray(lwr_chain_.getNrOfJoints());
return true;
}
bool TaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
nh_ = n;
// get URDF and name of root and tip from the parameter server
std::string robot_description, root_name, tip_name;
if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
return false;
}
if (!nh_.getParam("root_name", root_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
return false;
}
if (!nh_.getParam("tip_name", tip_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
return false;
}
ROS_INFO("robot_description: %s",robot_description.c_str());
ROS_INFO("root_name: %s",root_name.c_str());
ROS_INFO("tip_name: %s",tip_name.c_str());
// Get the gravity vector (direction and magnitude)
KDL::Vector gravity_ = KDL::Vector::Zero();
gravity_(2) = -9.81;
// Construct an URDF model from the xml string
std::string xml_string;
if (n.hasParam(robot_description))
n.getParam(robot_description.c_str(), xml_string);
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
n.shutdown();
return false;
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
n.shutdown();
return false;
}
ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());
// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n.shutdown();
return false;
}
ROS_DEBUG("Successfully parsed urdf file");
KDL::Tree kdl_tree_;
if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
{
ROS_ERROR("Failed to construct kdl tree");
n.shutdown();
return false;
}
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());
int index;
std::cout<< "kdl_chain_.getNrOfJoints(): " << kdl_chain_.getNrOfJoints() << std::endl;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
ROS_INFO("Successfully parsed joint limits");
// Get joint handles for all of the joints in the chain
for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
{
joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
ROS_DEBUG("%s", it->getJoint().getName().c_str() );
}
ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );
PIDs_.resize(kdl_chain_.getNrOfJoints());
// Parsing PID gains from YAML
std::string pid_ = ("pid_");
for (int i = 0; i < joint_handles_.size(); ++i)
{
if (!PIDs_[i].init(ros::NodeHandle(n, pid_ + joint_handles_[i].getName())))
{
ROS_ERROR("Error initializing the PID for joint %d",i);
return false;
}
}
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));
joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
joint_des_states_.resize(kdl_chain_.getNrOfJoints());
tau_cmd_.resize(kdl_chain_.getNrOfJoints());
J_.resize(kdl_chain_.getNrOfJoints());
sub_command_ = nh_.subscribe("command_configuration", 1, &TaskInverseKinematics::command_configuration, this);
return true;
}
