bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR_NAMED("kdl","kinematics not active");
return false;
}
poses.resize(link_names.size());
if(joint_angles.size() != dimension_)
{
ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
return false;
}
KDL::Frame p_out;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
KDL::JntArray jnt_pos_in(dimension_);
for(unsigned int i=0; i < dimension_; i++)
{
jnt_pos_in(i) = joint_angles[i];
}
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
bool valid = true;
for(unsigned int i=0; i < poses.size(); i++)
{
ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
{
tf::poseKDLToMsg(p_out,poses[i]);
}
else
{
ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
valid = false;
}
}
return valid;
}
bool NavfnWithLocalCostmap::setCostmap(SetCostmap::Request& req, SetCostmap::Response& resp)
{
cmap_.reset(new Costmap2D(req.width, req.height, 1.0, 0.0, 0.0));
unsigned ind=0;
for (unsigned y=0; y<req.height; y++)
for (unsigned x=0; x<req.width; x++)
cmap_->setCost(x, y, req.costs[ind++]);
for (unsigned y=0; y<req.height; y++)
for (unsigned x=0; x<req.width; x++)
ROS_DEBUG_NAMED ("node", "Cost of %u, %u is %u", x, y, cmap_->getCost(x,y));
planner_.reset(new NavFn(cmap_->getSizeInCellsX(), cmap_->getSizeInCellsY()));
ROS_DEBUG_STREAM_NAMED ("node", "Resetting planner object to have size " << cmap_->getSizeInCellsX() << ", " << cmap_->getSizeInCellsY());
return true;
}
/*! Disable collisions between end-effector and target
Disables ALL collisions on moved obstacles
*/
arm_navigation_msgs::OrderedCollisionOperations
GraspTesterWithApproach::collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp)
{
arm_navigation_msgs::OrderedCollisionOperations ord;
arm_navigation_msgs::CollisionOperation coll;
coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
if (!pickup_goal.collision_object_name.empty())
{
coll.object2 = pickup_goal.collision_object_name;
ord.collision_operations.push_back(coll);
}
if (pickup_goal.allow_gripper_support_collision)
{
coll.object2 = pickup_goal.collision_support_surface_name;
ord.collision_operations.push_back(coll);
}
coll.object1 = pickup_goal.collision_object_name;
//This disables all collisions with the object. Ignore possible collisions of the robot with the object
//during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable
//collisions just with the forearm, which should work almost always but still theoretially is not the right
//thing to do.
coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
ord.collision_operations.push_back(coll);
for (unsigned int i = 0; i < grasp.moved_obstacles.size(); i++)
{
ROS_DEBUG_NAMED("manipulation"," Disabling all collisions for grasp against moved obstacle %s",
grasp.moved_obstacles[i].collision_name.c_str());
coll.object1 = grasp.moved_obstacles[i].collision_name;
//This disables all collisions with the object. Ignore possible collisions of the robot with the object
//during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable
//collisions just with the forearm, which should work almost always but still theoretially is not the right
//thing to do.
coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
ord.collision_operations.push_back(coll);
}
ord.collision_operations = concat(ord.collision_operations,
pickup_goal.additional_collision_operations.collision_operations);
return ord;
}
void TransportSubscriberLink::enqueueMessage(const ros::SerializedMessage& m, bool ser, bool nocopy)
{
if (!ser)
{
return;
}
{
boost::mutex::scoped_lock lock(outbox_mutex_);
int max_queue = 0;
if (PublicationPtr parent = parent_.lock())
{
max_queue = parent->getMaxQueue();
}
ROS_DEBUG_NAMED("superdebug", "TransportSubscriberLink on topic [%s] to caller [%s], queueing message (queue size [%d])", topic_.c_str(), destination_caller_id_.c_str(), (int)outbox_.size());
if (max_queue > 0 && (int)outbox_.size() >= max_queue)
{
if (!queue_full_)
{
ROS_DEBUG("Outgoing queue full for topic [%s]. "
"Discarding oldest message\n",
topic_.c_str());
}
outbox_.pop(); // toss out the oldest thing in the queue to make room for us
queue_full_ = true;
}
else
{
queue_full_ = false;
}
outbox_.push(m);
}
startMessageWrite(false);
stats_.messages_sent_++;
stats_.bytes_sent_ += m.num_bytes;
stats_.message_data_sent_ += m.num_bytes;
}
void leatherman::setLoggerLevel(std::string name, std::string level)
{
ROSCONSOLE_AUTOINIT;
//std::string logger_name = ROSCONSOLE_DEFAULT_NAME + std::string(".") + name;
std::string logger_name = name;
ROS_INFO("Setting %s to %s level", logger_name.c_str(), level.c_str());
log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(logger_name);
// Set the logger for this package to output all statements
if(level.compare("debug") == 0)
my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
else
my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
ROS_DEBUG_NAMED(name, "This is a debug statement, and should print if you enabled debug.");
}
void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal)
{
planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
ROS_DEBUG_NAMED("manipulation", "Using default grasp poses");
goal.minimize_object_distance = true;
// add a number of default grasp points
// \todo add more!
moveit_msgs::Grasp g;
g.grasp_pose.header.frame_id = goal.target_name;
g.grasp_pose.pose.position.x = -0.2;
g.grasp_pose.pose.position.y = 0.0;
g.grasp_pose.pose.position.z = 0.0;
g.grasp_pose.pose.orientation.x = 0.0;
g.grasp_pose.pose.orientation.y = 0.0;
g.grasp_pose.pose.orientation.z = 0.0;
g.grasp_pose.pose.orientation.w = 1.0;
g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame();
g.pre_grasp_approach.direction.vector.x = 1.0;
g.pre_grasp_approach.min_distance = 0.1;
g.pre_grasp_approach.desired_distance = 0.2;
g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame();
g.post_grasp_retreat.direction.vector.z = 1.0;
g.post_grasp_retreat.min_distance = 0.1;
g.post_grasp_retreat.desired_distance = 0.2;
if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
{
g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
g.pre_grasp_posture.points.resize(1);
g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(),
std::numeric_limits<double>::max());
g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names;
g.grasp_posture.points.resize(1);
g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits<double>::max());
}
goal.possible_grasps.push_back(g);
}
ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedPlanningContext* pc,
kinematic_constraints::KinematicConstraintSetPtr ks,
constraint_samplers::ConstraintSamplerPtr cs)
: ob::GoalLazySamples(pc->getOMPLSimpleSetup()->getSpaceInformation(),
std::bind(&ConstrainedGoalSampler::sampleUsingConstraintSampler, this, std::placeholders::_1,
std::placeholders::_2),
false)
, planning_context_(pc)
, kinematic_constraint_set_(std::move(ks))
, constraint_sampler_(std::move(cs))
, work_state_(pc->getCompleteInitialRobotState())
, invalid_sampled_constraints_(0)
, warned_invalid_samples_(false)
, verbose_display_(0)
{
if (!constraint_sampler_)
default_sampler_ = si_->allocStateSampler();
ROS_DEBUG_NAMED("constrained_goal_sampler", "Constructed a ConstrainedGoalSampler instance at address %p", this);
startSampling();
}
void initialize(UAS &uas_)
{
uas = &uas_;
XmlRpc::XmlRpcValue map_dict;
if (!dist_nh.getParam("", map_dict)) {
ROS_WARN_NAMED("distance_sensor", "DS: plugin not configured!");
return;
}
ROS_ASSERT(map_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);
for (auto &pair : map_dict) {
ROS_DEBUG_NAMED("distance_sensor", "DS: initializing mapping for %s", pair.first.c_str());
auto it = DistanceSensorItem::create_item(this, pair.first);
if (it)
sensor_map[it->sensor_id] = it;
else
ROS_ERROR_NAMED("distance_sensor", "DS: bad config for %s", pair.first.c_str());
}
}
std::vector<int> GlobalFeatureRepositoryRecognizer::searchImpl(std::string id, std::vector<SensorDataPtr> data)
{
std::vector<int> res;
for (auto d : data) {
if (d->type_ == graph_slam_msgs::SensorData::SENSOR_TYPE_FEATURE) {
FeatureDataPtr feature_data = boost::dynamic_pointer_cast<FeatureData>(d);
cv::Mat descriptors = feature_data->features_;
std::vector<int> all_matches(place_count_ + 1, 0);
gfr_.match(descriptors, lmnDescInRep, all_matches, feature_data->feature_type_, config_.T);
// Get all matches that have more than T similarity.
std::vector<std::pair<int, float> > matches;
for (int i = 0; i < (int)all_matches.size(); i++) {
if (all_matches[i] > 0) {
if (all_matches[i] >= config_.T) {
matches.push_back(std::make_pair(i, (float)all_matches[i]));
}
}
}
ROS_DEBUG_NAMED("place recognition", "%d matches", (int) matches.size());
if (matches.size() > 0) {
// Sort matches.
res.reserve(matches.size());
std::sort(matches.begin(), matches.end(), CorrespondenceGreater());
for (auto match : matches) {
res.push_back(match.first);
}
}
}
}
return res;
}
GraspResult
SimpleGraspExecutor::executeGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp)
{
//demo_synchronizer::getClient().sync(2, "Using motion planner to move the arm to grasp position");
//demo_synchronizer::getClient().rviz(1, "Collision models;Planning goal;Collision map;Environment contacts");
mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP, -1);
if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name, ik_response_.solution.joint_state.position,
pickup_goal.additional_collision_operations,
pickup_goal.additional_link_padding) )
{
ROS_DEBUG_NAMED("manipulation"," Grasp execution: move arm reports failure");
if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.5, 0.0); //orange-ish
return Result(GraspResult::MOVE_ARM_FAILED, true);
}
mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP, pickup_goal.max_contact_force);
if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 0.0); //green
return Result(GraspResult::SUCCESS, true);
}
void CostMapAccessor::updateForRobotPosition(double wx, double wy){
if(wx < 0 || wx > costMap_.getResolution() * costMap_.getWidth())
return;
if(wy < 0 || wy > costMap_.getResolution() * costMap_.getHeight())
return;
origin_x_ = computeWX(costMap_, maxSize_, wx, wy);
origin_y_ = computeWY(costMap_, maxSize_, wx, wy);
costMap_.WC_MC(origin_x_, origin_y_, mx_0_, my_0_);
ROS_DEBUG_NAMED("costmap_2d",
"Moving map to locate at <%f, %f> and size of %f meters for position <%f, %f>\n",
origin_x_, origin_y_, maxSize_, wx, wy);
// Now update all the cells from the cost map
for(unsigned int x = 0; x < width_; x++){
for (unsigned int y = 0; y < height_; y++){
unsigned int ind = x + y * width_;
unsigned int global_ind = mx_0_ + x + (my_0_ + y) * costMap_.getWidth();
costData_[ind] = costMap_[global_ind];
}
}
}
void message_rx_cb(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_gps_raw_int_t raw_gps;
mavlink_msg_gps_raw_int_decode(msg, &raw_gps);
sensor_msgs::NavSatFixPtr fix(new sensor_msgs::NavSatFix);
geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped);
gps_diag.set_gps_raw(raw_gps);
if (raw_gps.fix_type < 2) {
ROS_WARN_THROTTLE_NAMED(60, "gps", "GPS: no fix");
return;
}
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type == 2 || raw_gps.fix_type == 3)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
fix->latitude = raw_gps.lat / 1E7; // deg
fix->longitude = raw_gps.lon / 1E7; // deg
fix->altitude = raw_gps.alt / 1E3; // m
if (raw_gps.eph != UINT16_MAX) {
double hdop = raw_gps.eph / 1E2;
double hdop2 = std::pow(hdop, 2);
// TODO: Check
// From nmea_navsat_driver
fix->position_covariance[0] = hdop2;
fix->position_covariance[4] = hdop2;
fix->position_covariance[8] = pow(2 * hdop, 2);
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}
fix->header.frame_id = frame_id;
fix->header.stamp = ros::Time::now();
fix_pub.publish(fix);
if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = raw_gps.cog / 1E2; // deg
// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);
vel->header.frame_id = frame_id;
vel->header.stamp = fix->header.stamp;
vel_pub.publish(vel);
}
}
break;
case MAVLINK_MSG_ID_GPS_STATUS:
{
// TODO: not supported by APM:Plane,
// no standard ROS messages
mavlink_gps_status_t gps_stat;
mavlink_msg_gps_status_decode(msg, &gps_stat);
ROS_DEBUG_NAMED("gps", "GPS stat sat visible: %d", gps_stat.satellites_visible);
}
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
{
mavlink_system_time_t mtime;
mavlink_msg_system_time_decode(msg, &mtime);
if (mtime.time_unix_usec == 0) {
ROS_WARN_THROTTLE_NAMED(60, "gps", "Wrong system time. Is GPS Ok? (boot_ms: %u)",
mtime.time_boot_ms);
return;
}
sensor_msgs::TimeReferencePtr time(new sensor_msgs::TimeReference);
ros::Time time_ref(
mtime.time_unix_usec / 1000000, // t_sec
(mtime.time_unix_usec % 1000000) * 1000); // t_nsec
time->source = time_ref_source;
time->time_ref = time_ref;
time->header.frame_id = time_ref_source;
time->header.stamp = ros::Time::now();
time_ref_pub.publish(time);
}
break;
};
}
DistanceSensorItem::Ptr DistanceSensorItem::create_item(DistanceSensorPlugin *owner, std::string topic_name)
{
auto p = boost::make_shared<DistanceSensorItem>();
std::string orientation_str;
ros::NodeHandle pnh(owner->dist_nh, topic_name);
p->owner = owner;
p->topic_name = topic_name;
// load and parse params
// first decide the type of topic (sub or pub)
pnh.param("subscriber", p->is_subscriber, false);
// sensor id
int id;
if (!pnh.getParam("id", id)) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: `id` not set!", topic_name.c_str());
p.reset(); return p; // return nullptr because of a bug related to gcc 4.6
}
p->sensor_id = id;
// orientation, checks later
if (!pnh.getParam("orientation", orientation_str))
p->orientation = -1; // not set
else
// lookup for numeric value
p->orientation = UAS::orientation_from_str(orientation_str);
if (!p->is_subscriber) {
// publisher params
// frame_id and FOV is required
if (!pnh.getParam("frame_id", p->frame_id)) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: `frame_id` not set!", topic_name.c_str());
p.reset(); return p; // nullptr
}
if (!pnh.getParam("field_of_view", p->field_of_view)) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: sensor FOV not set!", topic_name.c_str());
p.reset(); return p; // nullptr
}
// unset allowed, setted wrong - not
if (p->orientation == -1 && !orientation_str.empty()) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: defined orientation (%s) is not valid!",
topic_name.c_str(), orientation_str.c_str());
p.reset(); return p; // nullptr
}
// optional
pnh.param("send_tf", p->send_tf, false);
if (p->send_tf) { // sensor position defined if 'send_tf' set to TRUE
pnh.param("sensor_position/x", p->position.x(), 0.0);
pnh.param("sensor_position/y", p->position.y(), 0.0);
pnh.param("sensor_position/z", p->position.z(), 0.0);
ROS_DEBUG_NAMED("sensor_position", "DS: %s: Sensor position at: %f, %f, %f", topic_name.c_str(),
p->position.x(), p->position.y(), p->position.z());
}
}
else {
// subscriber params
// orientation is required
if (orientation_str.empty()) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: orientation not set!", topic_name.c_str());
p.reset(); return p; // nullptr
}
else if (p->orientation == -1) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: defined orientation (%s) is not valid!", topic_name.c_str(), orientation_str.c_str());
p.reset(); return p; // nullptr
}
// optional
pnh.param("covariance", p->covariance, 0);
}
// create topic handles
if (!p->is_subscriber)
p->pub = owner->dist_nh.advertise<sensor_msgs::Range>(topic_name, 10);
else
p->sub = owner->dist_nh.subscribe(topic_name, 10, &DistanceSensorItem::range_cb, p.get());
return p;
}
// Display the joint variable controls for the active joint group. Joint variable
// controls will be displayed if the following conditions are met:
//
// (1) A robot model has been loaded into the associated RobotCommandModel.
// (2) An active joint group is selected. This should always be true if there
// is at least one group in the robot model.
//
// In this scenario, we assume that the controls have already been set up (when
// we receive a robotLoaded() signal), and we simply need to add the controls to
// the layout and display them.
void JointVariableCommandWidget::updateJointGroupControls()
{
ROS_DEBUG_NAMED(LOG, "Display joint variable controls for active joint group");
auto& robot_model = m_model->getRobotModel();
if (!robot_model) {
ROS_DEBUG_NAMED(LOG, " -> Robot has not been loaded");
return;
}
if (m_active_joint_group.empty()) {
ROS_DEBUG_NAMED(LOG, " -> No active joint group set");
return;
}
auto* jmg = robot_model->getJointModelGroup(m_active_joint_group);
if (!jmg) {
// NOTE: This should always be true, since we check for existence of
// the active joint group
ROS_DEBUG_NAMED(LOG, " -> Joint Group '%s' does not exist within Robot Model '%s'", m_active_joint_group.c_str(), robot_model->getName().c_str());
return;
}
auto* glayout = qobject_cast<QGridLayout*>(layout());
assert(glayout != NULL);
////////////////////////////////////////
// Clear widgets from from the layout //
////////////////////////////////////////
ROS_DEBUG_NAMED(LOG, " Remove everything from layout");
while (!glayout->isEmpty()) {
auto* item = glayout->takeAt(0);
if (item->widget()) {
item->widget()->hide();
glayout->removeWidget(item->widget());
} else {
ROS_WARN("Here there be layouts?!");
glayout->removeItem(item);
delete item;
}
}
// Add the master label and group combo box back in.
glayout->addWidget(new QLabel(tr("Joint Group:")), 0, 0);
glayout->addWidget(m_joint_groups_combo_box, 0, 1);
m_joint_groups_combo_box->show(); // this gets hidden above
// Add the named state combo box in if we have named states for this group.
if (!jmg->getDefaultStateNames().empty()) {
glayout->addWidget(new QLabel(tr("Named State:")), 1, 0);
glayout->addWidget(m_joint_group_states_combo_box, 1, 1);
m_joint_group_states_combo_box->show();
}
/////////////////////////////////
// Add joint variable controls //
/////////////////////////////////
ROS_DEBUG_NAMED(LOG, " Add joint group controls to layout");
// gather (label, spinbox) pairs to be visible in the layout
// TODO: this could probably be made more better since spinboxes are
// constructed per-joint and should be able to be looked up from the joint
// they derive from
std::vector<std::pair<QLabel*, QDoubleSpinBox*>> group_widgets;
auto& joint_models = jmg->getActiveJointModels();
for (auto* joint_model : joint_models) {
auto bidx = joint_model->getFirstVariableIndex();
for (auto vidx = bidx; vidx < bidx + joint_model->getVariableCount(); ++vidx) {
auto& var_name = robot_model->getVariableNames()[vidx];
assert(vidx >= 0 && vidx < m_vind_to_spinbox.size());
for (auto* spinbox : m_vind_to_spinbox[vidx]) {
// find the label corresponding to this spinbox
auto it = std::find(m_spinboxes.begin(), m_spinboxes.end(), spinbox);
auto sidx = std::distance(m_spinboxes.begin(), it);
group_widgets.emplace_back(m_labels[sidx], spinbox);
}
}
}
// erase duplicate entries...see above for insight to why this is needed
auto uit = std::unique(group_widgets.begin(), group_widgets.end());
group_widgets.erase(uit, group_widgets.end());
// add (label, spinbox) widgets to the layout
auto row = glayout->rowCount();
for (auto& entry : group_widgets) {
ROS_DEBUG_NAMED(LOG, " Add spinbox %p and label %p to layout", entry.first, entry.second);
glayout->addWidget(entry.first, row, 0);
glayout->addWidget(entry.second, row, 1);
entry.first->show();
entry.second->show();
++row;
}
ROS_DEBUG_NAMED(LOG, " Layout contains %d controls", glayout->count());
///////////////////////////////////////////////////////////////////////
// KEEP THESE CALLS BECAUSE THEY USED TO BE THE ONLY WAY TO AUTOMATIC
// RESIZING WORK AND THEY HAVEN'T BROKEN YET
// for (int i = 0; i < glayout->count(); ++i) {
// glayout->itemAt(i)->invalidate();
// }
//
// glayout->invalidate();
layout()->invalidate();
// glayout->activate();
// layout()->activate();
// glayout->update();
layout()->update();
//////////////////////////////////////////////////////////////////////
}
bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
const std::vector<double> &ik_seed_state,
double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options) const
{
// Check if active
if(!active_)
{
ROS_ERROR_NAMED("srv","kinematics not active");
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
// Check if seed state correct
if(ik_seed_state.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("srv","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
// Check that we have the same number of poses as tips
if (tip_frames_.size() != ik_poses.size())
{
ROS_ERROR_STREAM_NAMED("srv","Mismatched number of pose requests (" << ik_poses.size()
<< ") to tip frames (" << tip_frames_.size() << ") in searchPositionIK");
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
// Create the service message
moveit_msgs::GetPositionIK ik_srv;
ik_srv.request.ik_request.avoid_collisions = true;
ik_srv.request.ik_request.group_name = getGroupName();
// Copy seed state into virtual robot state and convert into moveit_msg
robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state);
moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state);
// Load the poses into the request in difference places depending if there is more than one or not
geometry_msgs::PoseStamped ik_pose_st;
ik_pose_st.header.frame_id = base_frame_;
if (tip_frames_.size() > 1)
{
// Load into vector of poses
for (std::size_t i = 0; i < tip_frames_.size(); ++i)
{
ik_pose_st.pose = ik_poses[i];
ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st);
ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]);
}
}
else
{
ik_pose_st.pose = ik_poses[0];
// Load into single pose value
ik_srv.request.ik_request.pose_stamped = ik_pose_st;
ik_srv.request.ik_request.ik_link_name = getTipFrames()[0];
}
ROS_DEBUG_STREAM_NAMED("srv","Calling service: " << ik_service_client_->getService() );
if (ik_service_client_->call(ik_srv))
{
// Check error code
error_code.val = ik_srv.response.error_code.val;
if(error_code.val != error_code.SUCCESS)
{
ROS_DEBUG_NAMED("srv","An IK that satisifes the constraints and is collision free could not be found.");
switch (error_code.val)
{
// Debug mode for failure:
ROS_DEBUG_STREAM("Request was: \n" << ik_srv.request.ik_request);
ROS_DEBUG_STREAM("Response was: \n" << ik_srv.response.solution);
case moveit_msgs::MoveItErrorCodes::FAILURE:
ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: FAILURE");
break;
case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: NO IK SOLUTION");
break;
default:
ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: " << error_code.val);
}
return false;
}
}
else
{
ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService() );
error_code.val = error_code.FAILURE;
return false;
}
// Convert the robot state message to our robot_state representation
if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_))
{
ROS_ERROR_STREAM_NAMED("srv","An error occured converting recieved robot state message into internal robot state.");
error_code.val = error_code.FAILURE;
return false;
}
// Get just the joints we are concerned about in our planning group
robot_state_->copyJointGroupPositions(joint_model_group_, solution);
// Run the solution callback (i.e. collision checker) if available
if (!solution_callback.empty())
{
ROS_DEBUG_STREAM_NAMED("srv","Calling solution callback on IK solution");
// hack: should use all poses, not just the 0th
solution_callback(ik_poses[0], solution, error_code);
if(error_code.val != error_code.SUCCESS)
{
switch (error_code.val)
{
case moveit_msgs::MoveItErrorCodes::FAILURE:
ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: FAILURE");
break;
case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: NO IK SOLUTION");
break;
default:
ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: " << error_code.val);
}
return false;
}
}
ROS_INFO_STREAM_NAMED("srv","IK Solver Succeeded!");
return true;
}
bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
{
setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model || !srdf)
{
ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
return false;
}
robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
if (!joint_model_group)
return false;
if(!joint_model_group->isChain())
{
ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
return false;
}
if(!joint_model_group->isSingleDOFJoints())
{
ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
return false;
}
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
{
ROS_ERROR_NAMED("kdl","Could not initialize tree object");
return false;
}
if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
{
ROS_ERROR_NAMED("kdl","Could not initialize chain object");
return false;
}
dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
{
if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
{
ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
}
}
fk_chain_info_.joint_names = ik_chain_info_.joint_names;
fk_chain_info_.limits = ik_chain_info_.limits;
if(!joint_model_group->hasLinkModel(getTipFrame()))
{
ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
return false;
}
ik_chain_info_.link_names.push_back(getTipFrame());
fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
joint_min_.resize(ik_chain_info_.limits.size());
joint_max_.resize(ik_chain_info_.limits.size());
for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
{
joint_min_(i) = ik_chain_info_.limits[i].min_position;
joint_max_(i) = ik_chain_info_.limits[i].max_position;
}
// Get Solver Parameters
int max_solver_iterations;
double epsilon;
bool position_ik;
private_handle.param("max_solver_iterations", max_solver_iterations, 500);
private_handle.param("epsilon", epsilon, 1e-5);
private_handle.param(group_name+"/position_only_ik", position_ik, false);
ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
private_handle.getNamespace().c_str(),
(group_name+"/position_only_ik").c_str());
if(position_ik)
ROS_INFO_NAMED("kdl","Using position only ik");
num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
// Check for mimic joints
std::vector<unsigned int> redundant_joints_map_index;
std::vector<JointMimic> mimic_joints;
unsigned int joint_counter = 0;
for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
{
const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
//first check whether it belongs to the set of active joints in the group
if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
{
JointMimic mimic_joint;
mimic_joint.reset(joint_counter);
mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
mimic_joint.active = true;
mimic_joints.push_back(mimic_joint);
++joint_counter;
continue;
}
if (joint_model_group->hasJointModel(jm->getName()))
{
if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
{
JointMimic mimic_joint;
mimic_joint.reset(joint_counter);
mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
mimic_joint.offset = jm->getMimicOffset();
mimic_joint.multiplier = jm->getMimicFactor();
mimic_joints.push_back(mimic_joint);
continue;
}
}
}
for (std::size_t i = 0; i < mimic_joints.size(); ++i)
{
if(!mimic_joints[i].active)
{
const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
for(std::size_t j=0; j < mimic_joints.size(); ++j)
{
if(mimic_joints[j].joint_name == joint_model->getName())
{
mimic_joints[i].map_index = mimic_joints[j].map_index;
}
}
}
}
mimic_joints_ = mimic_joints;
// Setup the joint state groups that we need
state_.reset(new robot_state::RobotState(robot_model_));
state_2_.reset(new robot_state::RobotState(robot_model_));
// Store things for when the set of redundant joints may change
position_ik_ = position_ik;
joint_model_group_ = joint_model_group;
max_solver_iterations_ = max_solver_iterations;
epsilon_ = epsilon;
active_ = true;
ROS_DEBUG_NAMED("kdl","KDL solver initialized");
return true;
}
void sendCommad(){
ros::Duration sleep_time(0.1);
bool error = false;
unsigned int num_req;
while(nh_.ok() && ros::ok() && !error){
num_req = getRequestNum();
if (num_req > 0){
KukaRequest req = getRequest();
pop(); // TODO
// Check option
switch (req.opt_){
case STOP:
// Send Stop
error = kuka_.stop();
ROS_WARN_NAMED(name_, "Sending stop request to KUKA robot.");
break;
case PTP:
// Send PTP command
kuka_.setJointPosition(req.data_.ptp_goal);
ROS_DEBUG_NAMED(name_, "Sending PTP request to KUKA robot.");
break;
case VEL:
// Send PTP command
kuka_.setOverrideSpeed(req.data_.vel);
ROS_DEBUG_NAMED(name_, "Sending velocity override request to KUKA robot.");
break;
// case HOME:
// // Send Home
// for(unsigned int i=0; i<6; i++) req.data_.ptp_goal[i]=0.0*rad2deg+offset_[i];
// kuka_.setJointPosition(req.data_.ptp_goal);
// ROS_DEBUG_NAMED(name_, "Sending HOME request to KUKA robot.");
// break;
default:
ROS_ERROR_NAMED(name_, "Undefined option type.");
}
sleep_time = ros::Duration(0.1);
}
// Update joint state
else if (num_req == 0) {
ROS_DEBUG_NAMED(name_, "Update request to KUKA robot.");
// Call KUKA update
kuka_.update();
// Reset delta position and norm values
double norm = 0.0;
for (unsigned int i = 0; i < 6; ++i) position_d_[i] = joint_state_.position[i];
// Get joint position
kuka_.getJointPosition(joint_state_.position);
// Offset and rad convertion
for (unsigned int i = 0; i < 6; ++i)
{
joint_state_.position[i] = (joint_state_.position[i] - offset_[i])*deg2rad;
position_d_[i] -= joint_state_.position[i];
norm += position_d_[i]*position_d_[i];
}
//norm = sqrt(norm);
if ( norm < 0.01 ) sleep_time = ros::Duration(1);
ROS_DEBUG_NAMED(name_, "Norma: %.2f", norm);
// Publish joint state
joint_state_.header.stamp = ros::Time::now();
joint_state_.header.seq++;
joint_state_pub_.publish(joint_state_);
}
ROS_DEBUG_THROTTLE_NAMED(0.2, name_, "Current number of req %i", getRequestNum());
sleep_time.sleep();
}
}
void TOea_Planner::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal_msg)
{
if (!map_received_)
{
ROS_ERROR_NAMED(logger_name_, "No map received yet. Unable to compute path.");
return;
}
planner_state.data = hardware::BUSY; //PLANNING;
state_pub_.publish(planner_state);
ROS_DEBUG_NAMED(logger_name_, "New Goal received on topic");
// get world pose from msg
Astar_.goal_world_pose_.x = goal_msg->pose.position.x;
Astar_.goal_world_pose_.y = goal_msg->pose.position.y;
Astar_.goal_world_pose_.yaw = tf::getYaw(goal_msg->pose.orientation);
oea_msgs::Oea_path oea_path; // plan variable
// oea_path.path.poses.clear();
//check validity:
if (Astar_.goal_world_pose_.yaw!=Astar_.goal_world_pose_.yaw) //if nan
{
ROS_ERROR_STREAM_NAMED(logger_name_, "Invalig Goal: yaw is " << to_degrees(Astar_.goal_world_pose_.yaw));
//else just publish the blank path
oea_path_pub_.publish(oea_path); //publishing 0 poses will cause the controller to stop following the previous path
visual_path_pub_.publish(oea_path.path); // publish nav_msgs/Path to view path on rviz
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
return;
}
//convert it to grid coord
Astar_.ConvertWorlCoordToMatrix(Astar_.goal_world_pose_.x, Astar_.goal_world_pose_.y, Astar_.goal_world_pose_.yaw , Astar_.goal_grid_pose_.x, Astar_.goal_grid_pose_.y, Astar_.goal_grid_pose_.z);
std::string error_str;
// check if goal is valid
//error_str = "GoalCb: check grid pose val";
if (Astar_.is_valid_point(Astar_.goal_grid_pose_,error_str))
{
//get path from the Astar...
executeCycle(error_str, oea_path);
}
else
{
ROS_WARN_STREAM_NAMED(logger_name_, error_str);
// no need to clear Grid, because Astar was not called
// publish empty plan to stop the robot in case it's moving and received a new point...
oea_path_pub_.publish(oea_path);
visual_path_pub_.publish(oea_path.path);
}
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
}
object_manipulation_msgs::GraspResult
UnsafeGraspPerformer::approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp,
GraspExecutionInfo &execution_info)
{
ROS_DEBUG_NAMED("manipulation", "executing unsafe grasp");
//compute the pre-grasp pose
//get the grasp pose in the right frame
geometry_msgs::PoseStamped grasp_pose_stamped;
grasp_pose_stamped.pose = grasp.grasp_pose;
grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
grasp_pose_stamped.header.stamp = ros::Time(0);
//use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp
geometry_msgs::Vector3Stamped direction;
direction.header.stamp = ros::Time::now();
direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
//make sure the input is normalized
geometry_msgs::Vector3Stamped direction_norm = direction;
direction_norm.vector = mechInterface().normalize(direction.vector);
//multiply the approach direction by the desired length and translate the grasp pose back along it
double desired_trajectory_length = fabs(grasp.desired_approach_distance);
direction_norm.vector.x *= desired_trajectory_length;
direction_norm.vector.y *= desired_trajectory_length;
direction_norm.vector.z *= desired_trajectory_length;
geometry_msgs::PoseStamped pregrasp_pose = mechInterface().translateGripperPose(direction_norm, grasp_pose_stamped,
pickup_goal.arm_name);
//move to the pre-grasp using the Cartesian controller
mechInterface().moveArmToPoseCartesian(pickup_goal.arm_name, pregrasp_pose, ros::Duration(15.0),
.0015, .01, 0.02, 0.16, 0.005, 0.087, 0.1,
execution_info.approach_trajectory_.points.front().positions);
//move to the pre-grasp hand posture
mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP, -1);
//if not using reactive grasping, execute the unnormalized interpolated trajectory from pre-grasp to grasp
if(!pickup_goal.use_reactive_execution)
{
mechInterface().attemptTrajectory(pickup_goal.arm_name, execution_info.approach_trajectory_, true);
mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP, pickup_goal.max_contact_force);
}
//otherwise, call reactive grasping
else
{
geometry_msgs::PoseStamped final_grasp_pose_stamped;
final_grasp_pose_stamped.pose = grasp.grasp_pose;
final_grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
final_grasp_pose_stamped.header.stamp = ros::Time(0);
object_manipulation_msgs::ReactiveGraspGoal reactive_grasp_goal;
reactive_grasp_goal.arm_name = pickup_goal.arm_name;
reactive_grasp_goal.target = pickup_goal.target;
reactive_grasp_goal.final_grasp_pose = final_grasp_pose_stamped;
reactive_grasp_goal.trajectory = execution_info.approach_trajectory_;
reactive_grasp_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
reactive_grasp_goal.pre_grasp_posture = grasp.pre_grasp_posture;
reactive_grasp_goal.grasp_posture = grasp.grasp_posture;
//give the reactive grasp 3 minutes to do its thing
ros::Duration timeout = ros::Duration(180.0);
mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_grasp_goal);
if ( !mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
{
ROS_ERROR(" Reactive grasp timed out");
return Result(GraspResult::GRASP_FAILED, false);
}
object_manipulation_msgs::ReactiveGraspResult reactive_grasp_result =
*mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).getResult();
if (reactive_grasp_result.manipulation_result.value != reactive_grasp_result.manipulation_result.SUCCESS)
{
ROS_ERROR("Reactive grasp failed with error code %d", reactive_grasp_result.manipulation_result.value);
return Result(GraspResult::GRASP_FAILED, false);
}
}
if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 0.0); //green
return Result(GraspResult::SUCCESS, true);
}
void DoorTraj::evaluate_abs(OWD::Trajectory::TrajControl &tc, double t) {
if (OWD::Kinematics::max_condition > 25) {
ROS_ERROR_NAMED("OpenDoor","Jacobian Psuedo-Inverse condition number reached %2f; aborting", OWD::Kinematics::max_condition);
runstate=ABORT;
return;
}
// a place to record log values for debugging
std::vector<double> data;
// First, call evaluate on the base class to see where we would want
// to be if we weren't also dealing with the door
TrajControl tc2(tc);
OWD::MacJointTraj::evaluate_abs(tc2,t);
SE3 traj_endpoint_pose = interpolate_ee_pose(tc2.q);
data.push_back(time); // time, column 1
data.insert(data.end(),tc.q.begin(), tc.q.end()); // current positions, col 2-8
data.insert(data.end(),tc2.q.begin(), tc2.q.end()); // traj positions, col 9-15
std::vector<double> pose;
pose.resize(16);
memcpy(&pose[0],(double *)traj_endpoint_pose,16*sizeof(double));
data.insert(data.end(),pose.begin(), pose.end()); // traj pose, col 16-31
// calculate how much the endpoint position changed
R3 traj_endpoint_movement = (R3)traj_endpoint_pose - last_traj_endpoint;
last_traj_endpoint = (R3) traj_endpoint_pose;
// update the endpoint position goal
endpoint_position_goal += traj_endpoint_movement;
// endpoint_position_goal = (R3) traj_endpoint_pose;
ROS_DEBUG_STREAM_NAMED("OpenDoor","Current traj endpoint: " << (R3)traj_endpoint_pose);
ROS_DEBUG_STREAM_NAMED("OpenDoor","Current actual endpoint: " << (R3)hybridplug->endpoint);
ROS_DEBUG_STREAM_NAMED("OpenDoor","New endpoint goal: " << endpoint_position_goal);
// calculate the endpoint position error
R3 endpoint_error = endpoint_position_goal - (R3)hybridplug->endpoint;
memcpy(&pose[0],(const double *)hybridplug->endpoint,16*sizeof(double));
data.insert(data.end(),pose.begin(), pose.end()); // actual pose, col 32-47
data.push_back(endpoint_error[0]); // endpoint error, col 48-50
data.push_back(endpoint_error[1]);
data.push_back(endpoint_error[2]);
// project the position error onto the pull direction
R3 world_PullDirection = (SO3)hybridplug->endpoint * PullDirection;
R3 endpoint_longitudinal_error = (endpoint_error * world_PullDirection) * world_PullDirection;
R3 endpoint_lateral_error = endpoint_error - endpoint_longitudinal_error;
if (OWD::Plugin::simulation) {
ROS_DEBUG_NAMED("OpenDoor","Net ee movement of %2.3f",endpoint_longitudinal_error.norm());
}
data.push_back(endpoint_lateral_error[0]); // lateral error cols 51-53
data.push_back(endpoint_lateral_error[1]);
data.push_back(endpoint_lateral_error[2]);
data.push_back(endpoint_longitudinal_error[0]); // longitudinal error cols 54-56
data.push_back(endpoint_longitudinal_error[1]);
data.push_back(endpoint_longitudinal_error[2]);
// subtract the lateral error from the goal
endpoint_position_goal -= endpoint_lateral_error;
// calculate the endpoint rotation error
so3 endpoint_rotation_error_so3 =(so3)((SO3)traj_endpoint_pose * (! (SO3)hybridplug->endpoint));
// rotation error in world frame
R3 endpoint_rotation_error = endpoint_rotation_error_so3.t() * endpoint_rotation_error_so3.w();
data.push_back(endpoint_rotation_error[0]); // rotational error cols 57-59
data.push_back(endpoint_rotation_error[1]);
data.push_back(endpoint_rotation_error[2]);
// combine the longitudinal translation error with the rotation error
R6 endpoint_correction(endpoint_longitudinal_error,endpoint_rotation_error);
OWD::JointPos joint_correction;
try {
if (OWD::Plugin::simulation) {
ROS_DEBUG_STREAM_NAMED("OpenDoor","Endpoint pos/rot correction of" << std::endl << endpoint_correction);
}
joint_correction =
hybridplug->JacobianPseudoInverse_times_vector(endpoint_correction);
// data.insert(data.end(),joint_correction.begin(),
// joint_correction.end()); // joint change to correct ep, col 60-66
if (OWD::Plugin::simulation) {
ROS_DEBUG_NAMED("OpenDoor","JacobianPsueoInverse condition number %3.3f",OWD::Kinematics::max_condition);
ROS_DEBUG_NAMED("OpenDoor","JacobianPsuedoInverse Matrix:");
for (int i=0; i<7; ++i) {
ROS_DEBUG_NAMED("OpenDoor"," [%3.3f %3.3f %3.3f %3.3f %3.3f %3.3f]",
OWD::Kinematics::Jacobian0PseudoInverse[0][i], OWD::Kinematics::Jacobian0PseudoInverse[1][i],
OWD::Kinematics::Jacobian0PseudoInverse[2][i], OWD::Kinematics::Jacobian0PseudoInverse[3][i],
OWD::Kinematics::Jacobian0PseudoInverse[4][i], OWD::Kinematics::Jacobian0PseudoInverse[5][i]);
}
ROS_DEBUG_NAMED("OpenDoor","Joint correction [%1.3f, %1.3f, %1.3f, %1.3f, %1.3f, %1.3f, %1.3f]",
joint_correction[0], joint_correction[1], joint_correction[2], joint_correction[3], joint_correction[4], joint_correction[5], joint_correction[6]);
ROS_DEBUG_NAMED("OpenDoor"," (length of of %2.3f)",
joint_correction.length());
}
} catch (const char *err) {
// no valid Jacobian, for whatever reason, so we won't get
// any endpoint correction this timestep
data.insert(data.end(),tc.q.size(),-1); // invalid joint change (col 60-66)
if (OWD::Plugin::simulation) {
ROS_WARN_NAMED("OpenDoor","No JacobianPseudoInverse available");
}
}
// Finally, project our joint error into the nullspace so that
// we can correct as much as we can without changing the endpoint
OWD::JointPos joint_error(tc2.q - tc.q);
try {
OWD::JointPos configuration_correction
= hybridplug->Nullspace_projection(joint_error);
data.insert(data.end(),configuration_correction.begin(),
configuration_correction.end()); // joint change to correct config, col 67-73 (was 58-64)
if (OWD::Plugin::simulation) {
ROS_DEBUG_NAMED("OpenDoor","Configuration correction of %2.3f",
configuration_correction.length());
}
if (joint_correction.size() > 0) {
joint_correction += configuration_correction;
} else {
joint_correction = configuration_correction;
}
} catch (const char *err) {
// don't worry about it
data.insert(data.end(),tc.q.size(),-1); // invalid joint change (col 67-73)
if (OWD::Plugin::simulation) {
ROS_WARN_NAMED("OpenDoor","Nullspace projection failed");
}
}
if (OWD::Plugin::simulation) {
ROS_DEBUG_NAMED("OpenDoor","Total joint correction of %2.3f",
joint_correction.length());
}
if (joint_correction.size() == tc.q.size()) {
tc.q += joint_correction;
}
data.insert(data.end(),tc.q.begin(), tc.q.end()); // target positions, col 74-80 (was 65-71)
recorder->add(data);
if (runstate == DONE) {
end_position = tc.q;
}
}
base_local_planner::Trajectory DWAPlanner::computeTrajectories(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel){
tf::Stamped<tf::Pose> robot_pose_tf;
geometry_msgs::PoseStamped robot_pose;
//compute the distance between the robot and the last point on the global_plan
costmap_ros_->getRobotPose(robot_pose_tf);
tf::poseStampedTFToMsg(robot_pose_tf, robot_pose);
double sq_dist = squareDist(robot_pose, global_plan_.back());
bool two_point_scoring = true;
if(sq_dist < forward_point_distance_ * forward_point_distance_)
ROS_INFO("single points scoring");
two_point_scoring = false;
//compute the feasible velocity space based on the rate at which we run
Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
max_vel[0] = std::min(max_vel_x_, vel[0] + acc_lim_[0] * sim_period_);
max_vel[1] = std::min(max_vel_y_, vel[1] + acc_lim_[1] * sim_period_);
max_vel[2] = std::min(max_vel_th_, vel[2] + acc_lim_[2] * sim_period_);
Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
min_vel[0] = std::max(min_vel_x_, vel[0] - dec_lim_[0] * sim_period_);
min_vel[1] = std::max(min_vel_y_, vel[1] - dec_lim_[1] * sim_period_);
min_vel[2] = std::max(min_vel_th_, vel[2] - dec_lim_[2] * sim_period_);
Eigen::Vector3f dv = Eigen::Vector3f::Zero();
//we want to sample the velocity space regularly
for(unsigned int i = 0; i < 3; ++i){
dv[i] = (max_vel[i] - min_vel[i]) / (std::max(1.0, double(vsamples_[i]) - 1));
}
//keep track of the best trajectory seen so far... we'll re-use two memeber vars for efficiency
base_local_planner::Trajectory* best_traj = &traj_one_;
best_traj->cost_ = -1.0;
base_local_planner::Trajectory* comp_traj = &traj_two_;
comp_traj->cost_ = -1.0;
Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
//ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel[0], max_vel[0], min_vel[1], max_vel[1], min_vel[2], max_vel[2]);
//ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel_x_, max_vel_x_, min_vel_y_, max_vel_y_, min_vel_th_, max_vel_th_);
//ROS_ERROR("dv %.2f %.2f %.2f", dv[0], dv[1], dv[2]);
for(VelocityIterator x_it(min_vel[0], max_vel[0], dv[0]); !x_it.isFinished(); x_it++){
vel_samp[0] = x_it.getVelocity();
for(VelocityIterator y_it(min_vel[1], max_vel[1], dv[1]); !y_it.isFinished(); y_it++){
vel_samp[1] = y_it.getVelocity();
for(VelocityIterator th_it(min_vel[2], max_vel[2], dv[2]); !th_it.isFinished(); th_it++){
vel_samp[2] = th_it.getVelocity();
generateTrajectory(pos, vel_samp, *comp_traj, two_point_scoring, sq_dist);
selectBestTrajectory(best_traj, comp_traj);
}
}
}
ROS_DEBUG_NAMED("oscillation_flags", "forward_pos_only: %d, forward_neg_only: %d, strafe_pos_only: %d, strafe_neg_only: %d, rot_pos_only: %d, rot_neg_only: %d",
forward_pos_only_, forward_neg_only_, strafe_pos_only_, strafe_neg_only_, rot_pos_only_, rot_neg_only_);
//ok... now we have our best trajectory
if(best_traj->cost_ >= 0){
//we want to check if we need to set any oscillation flags
if(setOscillationFlags(best_traj)){
prev_stationary_pos_ = pos;
}
//if we've got restrictions... check if we can reset any oscillation flags
if(forward_pos_only_ || forward_neg_only_
|| strafe_pos_only_ || strafe_neg_only_
|| rot_pos_only_ || rot_neg_only_){
resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);
}
}
//TODO: Think about whether we want to try to do things like back up when a valid trajectory is not found
return *best_traj;
}
bool SrvKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::vector<std::string>& tip_frames,
double search_discretization)
{
bool debug = false;
ROS_INFO_STREAM_NAMED("srv","SrvKinematicsPlugin initializing");
setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model || !srdf)
{
ROS_ERROR_NAMED("srv","URDF and SRDF must be loaded for SRV kinematics solver to work."); // TODO: is this true?
return false;
}
robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
joint_model_group_ = robot_model_->getJointModelGroup(group_name);
if (!joint_model_group_)
return false;
if (debug)
{
std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
std::cout << std::endl;
}
// Get the dimension of the planning group
dimension_ = joint_model_group_->getVariableCount();
ROS_INFO_STREAM_NAMED("srv","Dimension planning group '" << group_name << "': " << dimension_
<< ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
<< ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
// Copy joint names
for (std::size_t i=0; i < joint_model_group_->getJointModels().size(); ++i)
{
ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
}
if (debug)
{
ROS_ERROR_STREAM_NAMED("temp","tip links available:");
std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
}
// Make sure all the tip links are in the link_names vector
for (std::size_t i = 0; i < tip_frames_.size(); ++i)
{
if(!joint_model_group_->hasLinkModel(tip_frames_[i]))
{
ROS_ERROR_NAMED("srv","Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), group_name.c_str());
return false;
}
ik_group_info_.link_names.push_back(tip_frames_[i]);
}
// Choose what ROS service to send IK requests to
ROS_DEBUG_STREAM_NAMED("srv","Looking for ROS service name on rosparm server at location: " <<
private_handle.getNamespace() << "/" << group_name_ << "/kinematics_solver_service_name");
std::string ik_service_name;
private_handle.param(group_name_ + "/kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
// Setup the joint state groups that we need
robot_state_.reset(new robot_state::RobotState(robot_model_));
robot_state_->setToDefaultValues();
// Create the ROS service client
ros::NodeHandle nonprivate_handle("");
ik_service_client_ = boost::make_shared<ros::ServiceClient>(nonprivate_handle.serviceClient
<moveit_msgs::GetPositionIK>(ik_service_name));
if (!ik_service_client_->waitForExistence(ros::Duration(0.1))) // wait 0.1 seconds, blocking
ROS_WARN_STREAM_NAMED("srv","Unable to connect to ROS service client with name: " << ik_service_client_->getService());
else
ROS_INFO_STREAM_NAMED("srv","Service client started with ROS service name: " << ik_service_client_->getService());
active_ = true;
ROS_DEBUG_NAMED("srv","ROS service-based kinematics solver initialized");
return true;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIMU::Update()
{
// Get Time Difference dt
common::Time cur_time = world->GetSimTime();
double dt = (cur_time - last_time).Double();
if (last_time + update_period > cur_time) return;
boost::mutex::scoped_lock scoped_lock(lock);
// Get Pose/Orientation
math::Pose pose = link->GetWorldPose();
// get Acceleration and Angular Rates
// the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
//accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
accel = pose.rot.RotateVectorReverse((temp - velocity) / dt);
velocity = temp;
// GetRelativeAngularVel() sometimes return nan?
//rate = link->GetRelativeAngularVel(); // get angular rate in body frame
math::Quaternion delta = pose.rot - orientation;
orientation = pose.rot;
rate.x = 2.0 * (-orientation.x * delta.w + orientation.w * delta.x + orientation.z * delta.y - orientation.y * delta.z) / dt;
rate.y = 2.0 * (-orientation.y * delta.w - orientation.z * delta.x + orientation.w * delta.y + orientation.x * delta.z) / dt;
rate.z = 2.0 * (-orientation.z * delta.w + orientation.y * delta.x - orientation.x * delta.y + orientation.w * delta.z) / dt;
// get Gravity
gravity = world->GetPhysicsEngine()->GetGravity();
gravity_body = orientation.RotateVectorReverse(gravity);
double gravity_length = gravity.GetLength();
ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
// add gravity vector to body acceleration
accel = accel - gravity_body;
// update sensor models
accel = accel + accelModel.update(dt);
rate = rate + rateModel.update(dt);
headingModel.update(dt);
ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "Current errors: accel = [%g %g %g], rate = [%g %g %g], heading = %g",
accelModel.getCurrentError().x, accelModel.getCurrentError().y, accelModel.getCurrentError().z,
rateModel.getCurrentError().x, rateModel.getCurrentError().y, rateModel.getCurrentError().z,
headingModel.getCurrentError());
// apply offset error to orientation (pseudo AHRS)
double normalization_constant = (gravity_body + accelModel.getCurrentError()).GetLength() * gravity_body.GetLength();
double cos_alpha = (gravity_body + accelModel.getCurrentError()).Dot(gravity_body)/normalization_constant;
math::Vector3 normal_vector(gravity_body.Cross(accelModel.getCurrentError()));
normal_vector *= sqrt((1 - cos_alpha)/2)/normalization_constant;
math::Quaternion attitudeError(sqrt((1 + cos_alpha)/2), normal_vector.x, normal_vector.y, normal_vector.z);
math::Quaternion headingError(cos(headingModel.getCurrentError()/2),0,0,sin(headingModel.getCurrentError()/2));
pose.rot = attitudeError * pose.rot * headingError;
// copy data into pose message
imuMsg.header.frame_id = frameId;
imuMsg.header.stamp.sec = cur_time.sec;
imuMsg.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
imuMsg.orientation.x = pose.rot.x;
imuMsg.orientation.y = pose.rot.y;
imuMsg.orientation.z = pose.rot.z;
imuMsg.orientation.w = pose.rot.w;
// pass angular rates
imuMsg.angular_velocity.x = rate.x;
imuMsg.angular_velocity.y = rate.y;
imuMsg.angular_velocity.z = rate.z;
// pass accelerations
imuMsg.linear_acceleration.x = accel.x;
imuMsg.linear_acceleration.y = accel.y;
imuMsg.linear_acceleration.z = accel.z;
// fill in covariance matrix
imuMsg.orientation_covariance[8] = headingModel.gaussian_noise*headingModel.gaussian_noise;
if (gravity_length > 0.0) {
imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
} else {
imuMsg.orientation_covariance[0] = -1;
imuMsg.orientation_covariance[4] = -1;
}
// publish to ros
pub_.publish(imuMsg);
// debug output
#ifdef DEBUG_OUTPUT
if (debugPublisher) {
geometry_msgs::PoseStamped debugPose;
debugPose.header = imuMsg.header;
debugPose.header.frame_id = "/map";
debugPose.pose.orientation.w = imuMsg.orientation.w;
debugPose.pose.orientation.x = imuMsg.orientation.x;
debugPose.pose.orientation.y = imuMsg.orientation.y;
debugPose.pose.orientation.z = imuMsg.orientation.z;
math::Pose pose = link->GetWorldPose();
debugPose.pose.position.x = pose.pos.x;
debugPose.pose.position.y = pose.pos.y;
debugPose.pose.position.z = pose.pos.z;
debugPublisher.publish(debugPose);
}
#endif // DEBUG_OUTPUT
// save last time stamp
last_time = cur_time;
}
TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param, double _maxtime, double _eps, SolveType _type ) :
initialized(false),
eps(_eps),
maxtime(_maxtime),
solvetype(_type),
work(io_service)
{
ros::NodeHandle node_handle("~");
urdf::Model robot_model;
std::string xml_string;
std::string urdf_xml,full_urdf_xml;
node_handle.param("urdf_xml",urdf_xml,URDF_param);
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG_NAMED("trac_ik","Reading xml file from parameter server");
if (!node_handle.getParam(full_urdf_xml, xml_string))
{
ROS_FATAL_NAMED("trac_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
return;
}
node_handle.param(full_urdf_xml,xml_string,std::string());
robot_model.initString(xml_string);
ROS_DEBUG_STREAM_NAMED("trac_ik","Reading joints and links from URDF");
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree))
ROS_FATAL("Failed to extract kdl tree from xml robot description");
if(!tree.getChain(base_link, tip_link, chain))
ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
std::vector<KDL::Segment> chain_segs = chain.segments;
boost::shared_ptr<const urdf::Joint> joint;
std::vector<double> l_bounds, u_bounds;
lb.resize(chain.getNrOfJoints());
ub.resize(chain.getNrOfJoints());
uint joint_num=0;
for(unsigned int i = 0; i < chain_segs.size(); ++i) {
joint = robot_model.getJoint(chain_segs[i].getJoint().getName());
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
joint_num++;
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS ) {
if(joint->safety) {
lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
} else {
lower = joint->limits->lower;
upper = joint->limits->upper;
}
hasLimits = 1;
}
else {
hasLimits = 0;
}
if(hasLimits) {
lb(joint_num-1)=lower;
ub(joint_num-1)=upper;
}
else {
lb(joint_num-1)=std::numeric_limits<float>::lowest();
ub(joint_num-1)=std::numeric_limits<float>::max();
}
ROS_INFO_STREAM("IK Using joint "<<joint->name<<" "<<lb(joint_num-1)<<" "<<ub(joint_num-1));
}
}
initialize();
}
// Close end effector
bool closeEndEffector()
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check if end effector is already close and arm is stil
if( ee_status_.target_position == END_EFFECTOR_CLOSE_VALUE_MAX &&
ee_status_.moving == false &&
ee_status_.position > END_EFFECTOR_CLOSE_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_CLOSE_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the correct position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector already closed within tolerance, unable to close further");
return true;
}
// Set the velocity for the end effector to a low value
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity low");
velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
if( !velocity_client_.waitForExistence(ros::Duration(5.0)) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Timed out waiting for velocity client existance");
return false;
}
dynamixel_hardware_interface::SetVelocity set_velocity_srv;
set_velocity_srv.request.velocity = END_EFFECTOR_MEDIUM_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
std_msgs::Float64 joint_value;
double timeout_sec = 10.0; // time before we declare an error
const double CHECK_INTERVAL = 0.1; // how often we check the load
const double FIRST_BACKOUT_AMOUNT = -0.25;
const double SECOND_BACKOUT_AMOUNT = -0.0075;
const double BACKOUT_AMOUNT[2] = {FIRST_BACKOUT_AMOUNT, SECOND_BACKOUT_AMOUNT};
// Grasp twice - to reduce amount of slips and ensure better grip
for(int i = 0; i < 2; ++i)
{
timeout_sec = 10; // reset timeout;
ROS_DEBUG_STREAM("Grasping with end effector - grasp number " << i + 1);
// Tell servos to start closing slowly to max amount
joint_value.data = END_EFFECTOR_CLOSE_VALUE_MAX;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
while( ee_status_.position < joint_value.data - END_EFFECTOR_POSITION_TOLERANCE ||
ee_status_.position > joint_value.data + END_EFFECTOR_POSITION_TOLERANCE )
{
ros::spinOnce(); // Allows ros to get the latest servo message - we need the load
// Check if load has peaked
if( ee_status_.load < END_EFFECTOR_LOAD_SETPOINT ) // we have touched object!
{
// Open the gripper back up a little to reduce the amount of load.
// the first time open it a lot to help with grasp quality
joint_value.data = ee_status_.position + BACKOUT_AMOUNT[i];
// Check that we haven't passed the open limit
if( joint_value.data < END_EFFECTOR_OPEN_VALUE_MAX )
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
ROS_DEBUG_NAMED("clam_gripper_controller","Setting end effector setpoint to %f when it was %f", joint_value.data, ee_status_.position);
end_effector_pub_.publish(joint_value);
if( i == 0 ) // give lots of time to pull out the first time
{
ros::Duration(1.00).sleep();
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Sleeping as we publish joint value " << joint_value.data);
set_velocity_srv.request.velocity = END_EFFECTOR_SLOW_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
ros::Duration(1.0).sleep();
}
break;
}
// Debug output
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","" << joint_value.data - END_EFFECTOR_POSITION_TOLERANCE << " < " <<
ee_status_.position << " < " << joint_value.data + END_EFFECTOR_POSITION_TOLERANCE
<< " -- LOAD: " << ee_status_.load );
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
// Wait an interval before checking again
ros::Duration(CHECK_INTERVAL).sleep();
// Check if timeout has occured
timeout_sec -= CHECK_INTERVAL;
if( timeout_sec <= 0 )
{
ROS_ERROR_NAMED("clam_gripper_controller","Timeout: Unable to close end effector");
return false;
}
}
}
// DONE
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished closing end effector action");
return true;
}
unsigned int CostMapAccessor::computeSize(double maxSize, double resolution){
unsigned int cellWidth = (unsigned int) ceil(maxSize/resolution);
ROS_DEBUG_NAMED("costmap_2d", "Given a size of %f and a resolution of %f, we have a cell width of %d\n", maxSize, resolution, cellWidth);
return cellWidth;
}
bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const std::vector<double> &consistency_limits,
const kinematics::KinematicsQueryOptions &options) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR_NAMED("kdl","kinematics not active");
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
if(ik_seed_state.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
KDL::JntArray jnt_seed_state(dimension_);
KDL::JntArray jnt_pos_in(dimension_);
KDL::JntArray jnt_pos_out(dimension_);
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
ik_solver_vel.setMimicJoints(mimic_joints_);
ik_solver_pos.setMimicJoints(mimic_joints_);
if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
{
ROS_ERROR_NAMED("kdl","Could not set redundant joints");
return false;
}
if(options.lock_redundant_joints)
{
ik_solver_vel.lockRedundantJoints();
}
solution.resize(dimension_);
KDL::Frame pose_desired;
tf::poseMsgToKDL(ik_pose, pose_desired);
ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
ik_pose.position.x << " " <<
ik_pose.position.y << " " <<
ik_pose.position.z << " " <<
ik_pose.orientation.x << " " <<
ik_pose.orientation.y << " " <<
ik_pose.orientation.z << " " <<
ik_pose.orientation.w);
//Do the IK
for(unsigned int i=0; i < dimension_; i++)
jnt_seed_state(i) = ik_seed_state[i];
jnt_pos_in = jnt_seed_state;
unsigned int counter(0);
while(1)
{
// ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
counter++;
if(timedOut(n1,timeout))
{
ROS_DEBUG_NAMED("kdl","IK timed out");
error_code.val = error_code.TIMED_OUT;
ik_solver_vel.unlockRedundantJoints();
return false;
}
int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
if(!consistency_limits.empty())
{
getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
{
ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
continue;
}
}
else
{
getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
ROS_DEBUG_NAMED("kdl","New random configuration");
for(unsigned int j=0; j < dimension_; j++)
ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));
if(ik_valid < 0 && !options.return_approximate_solution)
{
ROS_DEBUG_NAMED("kdl","Could not find IK solution");
continue;
}
}
ROS_DEBUG_NAMED("kdl","Found IK solution");
for(unsigned int j=0; j < dimension_; j++)
solution[j] = jnt_pos_out(j);
if(!solution_callback.empty())
solution_callback(ik_pose,solution,error_code);
else
error_code.val = error_code.SUCCESS;
if(error_code.val == error_code.SUCCESS)
{
ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
ik_solver_vel.unlockRedundantJoints();
return true;
}
}
ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
error_code.val = error_code.NO_IK_SOLUTION;
ik_solver_vel.unlockRedundantJoints();
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosIMU::LoadThread()
{
// load parameters
this->robot_namespace_ = "";
if (this->sdf->HasElement("robotNamespace"))
this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
if (!this->sdf->HasElement("serviceName"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <serviceName>, defaults to /default_imu");
this->service_name_ = "/default_imu";
}
else
this->service_name_ = this->sdf->Get<std::string>("serviceName");
if (!this->sdf->HasElement("topicName"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <topicName>, defaults to /default_imu");
this->topic_name_ = "/default_imu";
}
else
this->topic_name_ = this->sdf->Get<std::string>("topicName");
if (!this->sdf->HasElement("gaussianNoise"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0.0;
}
else
this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
if (!this->sdf->HasElement("bodyName"))
{
ROS_FATAL_NAMED("imu", "imu plugin missing <bodyName>, cannot proceed");
return;
}
else
this->link_name_ = this->sdf->Get<std::string>("bodyName");
if (!this->sdf->HasElement("xyzOffset"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <xyzOffset>, defaults to 0s");
this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0);
}
else
this->offset_.Pos() = this->sdf->Get<ignition::math::Vector3d>("xyzOffset");
if (!this->sdf->HasElement("rpyOffset"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <rpyOffset>, defaults to 0s");
this->offset_.Rot() = ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0));
}
else
this->offset_.Rot() = ignition::math::Quaterniond(this->sdf->Get<ignition::math::Vector3d>("rpyOffset"));
if (!this->sdf->HasElement("updateRate"))
{
ROS_DEBUG_NAMED("imu", "imu plugin missing <updateRate>, defaults to 0.0"
" (as fast as possible)");
this->update_rate_ = 0.0;
}
else
this->update_rate_ = this->sdf->GetElement("updateRate")->Get<double>();
if (!this->sdf->HasElement("frameName"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <frameName>, defaults to <bodyName>");
this->frame_name_ = link_name_;
}
else
this->frame_name_ = this->sdf->Get<std::string>("frameName");
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("imu", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
// publish multi queue
this->pmq.startServiceThread();
// assert that the body by link_name_ exists
this->link = boost::dynamic_pointer_cast<physics::Link>(
this->world_->GetEntity(this->link_name_));
if (!this->link)
{
ROS_FATAL_NAMED("imu", "gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
this->link_name_.c_str());
return;
}
// if topic name specified as empty, do not publish
if (this->topic_name_ != "")
{
this->pub_Queue = this->pmq.addPub<sensor_msgs::Imu>();
this->pub_ = this->rosnode_->advertise<sensor_msgs::Imu>(
this->topic_name_, 1);
// advertise services on the custom queue
ros::AdvertiseServiceOptions aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback,
this, _1, _2), ros::VoidPtr(), &this->imu_queue_);
this->srv_ = this->rosnode_->advertiseService(aso);
}
// Initialize the controller
this->last_time_ = this->world_->GetSimTime();
// this->initial_pose_ = this->link->GetPose();
this->last_vpos_ = this->link->GetWorldLinearVel().Ign();
this->last_veul_ = this->link->GetWorldAngularVel().Ign();
this->apos_ = 0;
this->aeul_ = 0;
// start custom queue for imu
this->callback_queue_thread_ =
boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this));
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosIMU::UpdateChild, this));
}
bool DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel) {
// dynamic window sampling approach to get useful velocity commands
if(! isInitialized()){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
tf::Stamped<tf::Pose> robot_vel;
odom_helper_.getRobotVel(robot_vel);
/* For timing uncomment
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
*/
//compute what trajectory to drive along
tf::Stamped<tf::Pose> drive_cmds;
drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
// call with updated footprint
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
//ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);
/* For timing uncomment
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
ROS_INFO("Cycle time: %.9f", t_diff);
*/
//pass along drive commands
cmd_vel.linear.x = drive_cmds.getOrigin().getX();
cmd_vel.linear.y = drive_cmds.getOrigin().getY();
cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
//if we cannot move... tell someone
std::vector<geometry_msgs::PoseStamped> local_plan;
if(path.cost_ < 0) {
ROS_DEBUG_NAMED("dwa_local_planner",
"The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
local_plan.clear();
publishLocalPlan(local_plan);
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
// Fill out the local plan
for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
tf::Stamped<tf::Pose> p =
tf::Stamped<tf::Pose>(tf::Pose(
tf::createQuaternionFromYaw(p_th),
tf::Point(p_x, p_y, 0.0)),
ros::Time::now(),
costmap_ros_->getGlobalFrameID());
geometry_msgs::PoseStamped pose;
tf::poseStampedTFToMsg(p, pose);
local_plan.push_back(pose);
}
//publish information to the visualizer
publishLocalPlan(local_plan);
return true;
}
SNS_IK::SNS_IK(const std::string& base_link, const std::string& tip_link,
const std::string& URDF_param, double loopPeriod, double eps,
sns_ik::VelocitySolveType type) :
m_initialized(false),
m_eps(eps),
m_loopPeriod(loopPeriod),
m_nullspaceGain(1.0),
m_solvetype(type)
{
ros::NodeHandle node_handle("~");
urdf::Model robot_model;
std::string xml_string;
std::string urdf_xml, full_urdf_xml;
node_handle.param("urdf_param",urdf_xml,URDF_param);
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG_NAMED("sns_ik","Reading xml file from parameter server");
if (!node_handle.getParam(full_urdf_xml, xml_string)) {
ROS_FATAL_NAMED("sns_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
return;
}
node_handle.param(full_urdf_xml, xml_string, std::string());
robot_model.initString(xml_string);
ROS_DEBUG_STREAM_NAMED("sns_ik","Reading joints and links from URDF");
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
ROS_FATAL("Failed to extract kdl tree from xml robot description.");
return;
}
if(!tree.getChain(base_link, tip_link, m_chain)) {
ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
}
std::vector<KDL::Segment> chain_segments = m_chain.segments;
boost::shared_ptr<const urdf::Joint> joint;
m_lower_bounds.resize(m_chain.getNrOfJoints());
m_upper_bounds.resize(m_chain.getNrOfJoints());
m_velocity.resize(m_chain.getNrOfJoints());
m_acceleration.resize(m_chain.getNrOfJoints());
m_jointNames.resize(m_chain.getNrOfJoints());
unsigned int joint_num=0;
for(std::size_t i = 0; i < chain_segments.size(); ++i) {
joint = robot_model.getJoint(chain_segments[i].getJoint().getName());
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
double lower=0; //TODO Better default values? Error if these arent found?
double upper=0;
double velocity=0;
double acceleration=0;
if ( joint->type == urdf::Joint::CONTINUOUS ) {
lower=std::numeric_limits<float>::lowest();
upper=std::numeric_limits<float>::max();
} else {
if(joint->safety) {
lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
} else {
lower = joint->limits->lower;
upper = joint->limits->upper;
}
velocity = std::fabs(joint->limits->velocity);
}
// Checking the Param server for limit modifications
// and acceleration limits
std::string prefix = urdf_xml + "_planning/joint_limits/" + joint->name + "/";
double ul;
if(node_handle.getParam(prefix + "max_position", ul)){
upper = std::min(upper, ul);
}
double ll;
if(node_handle.getParam(prefix + "min_position", ll)){
lower = std::max(lower, ll);
}
double vel;
if(node_handle.getParam(prefix + "max_velocity", vel)){
if (velocity > 0)
velocity = std::min(velocity, std::fabs(vel));
else
velocity = std::fabs(vel);
}
node_handle.getParam(prefix + "max_acceleration", acceleration);
m_lower_bounds(joint_num)=lower;
m_upper_bounds(joint_num)=upper;
m_velocity(joint_num) = velocity;
m_acceleration(joint_num) = std::fabs(acceleration);
m_jointNames[joint_num] = joint->name;
ROS_INFO("sns_ik: Using joint %s lb: %.3f, ub: %.3f, v: %.3f, a: %.3f", joint->name.c_str(),
m_lower_bounds(joint_num), m_upper_bounds(joint_num), m_velocity(joint_num), m_acceleration(joint_num));
joint_num++;
}
}
initialize();
}
