void spin() {
// initialize random seed
srand(time(NULL));
// dynamics model
boost::shared_ptr<DynamicModel > dyn_model( new DynModelPlanar5() );
std::string robot_description_str;
std::string robot_semantic_description_str;
nh_.getParam("/robot_description", robot_description_str);
nh_.getParam("/robot_semantic_description", robot_semantic_description_str);
//
// collision model
//
boost::shared_ptr<self_collision::CollisionModel> col_model = self_collision::CollisionModel::parseURDF(robot_description_str);
col_model->parseSRDF(robot_semantic_description_str);
col_model->generateCollisionPairs();
// external collision objects - part of virtual link connected to the base link
self_collision::Link::VecPtrCollision col_array;
col_array.push_back( self_collision::createCollisionCapsule(0.2, 0.3, KDL::Frame(KDL::Rotation::RotX(90.0/180.0*PI), KDL::Vector(1, 0.5, 0))) );
if (!col_model->addLink("env_link", "base", col_array)) {
ROS_ERROR("ERROR: could not add external collision objects to the collision model");
return;
}
col_model->generateCollisionPairs();
//
// robot state
//
std::vector<std::string > joint_names;
joint_names.push_back("0_joint");
joint_names.push_back("1_joint");
joint_names.push_back("2_joint");
joint_names.push_back("3_joint");
joint_names.push_back("4_joint");
int ndof = joint_names.size();
Eigen::VectorXd q, dq, ddq, torque;
q.resize( ndof );
dq.resize( ndof );
ddq.resize( ndof );
torque.resize( ndof );
for (int q_idx = 0; q_idx < ndof; q_idx++) {
q[q_idx] = -0.1;
dq[q_idx] = 0.0;
ddq[q_idx] = 0.0;
torque[q_idx] = 0.0;
}
std::string effector_name = "effector";
int effector_idx = col_model->getLinkIndex(effector_name);
//
// kinematic model
//
boost::shared_ptr<KinematicModel> kin_model( new KinematicModel(robot_description_str, joint_names) );
KinematicModel::Jacobian J_r_HAND_6, J_r_HAND;
J_r_HAND_6.resize(6, ndof);
J_r_HAND.resize(3, ndof);
std::vector<KDL::Frame > links_fk(col_model->getLinksCount());
// joint limits
Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof);
int q_idx = 0;
for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) {
lower_limit[q_idx] = kin_model->getLowerLimit(q_idx);
upper_limit[q_idx] = kin_model->getUpperLimit(q_idx);
limit_range[q_idx] = 10.0/180.0*PI;
max_trq[q_idx] = 10.0;
}
Eigen::VectorXd max_q(ndof);
max_q(0) = 10.0/180.0*PI;
max_q(1) = 20.0/180.0*PI;
max_q(2) = 20.0/180.0*PI;
max_q(3) = 30.0/180.0*PI;
max_q(4) = 40.0/180.0*PI;
boost::shared_ptr<DynamicsSimulatorHandPose> sim( new DynamicsSimulatorHandPose(ndof, 6, effector_name, col_model, kin_model, dyn_model, joint_names, max_q) );
/*
//
// Tasks declaration
//
Task_JLC task_JLC(lower_limit, upper_limit, limit_range, max_trq);
Task_WCC task_WCC(ndof, 3, 4);
double activation_dist = 0.05;
Task_COL task_COL(ndof, activation_dist, 10.0, kin_model, col_model);
Task_HAND task_HAND(ndof, 3);
*/
/*
while (ros::ok()) {
//
// wrist collision constraint
//
Eigen::VectorXd torque_WCC(ndof);
Eigen::MatrixXd N_WCC(ndof, ndof);
task_WCC.compute(q, dq, dyn_model->getM(), dyn_model->getInvM(), torque_WCC, N_WCC, markers_pub_);
markers_pub_.publish();
ros::spinOnce();
char ch = getchar();
if (ch == 'a') {
q(3) -= 0.1;
}
else if (ch == 'd') {
q(3) += 0.1;
}
else if (ch == 's') {
q(4) -= 0.1;
}
else if (ch == 'w') {
q(4) += 0.1;
}
}
return;
*/
// loop variables
ros::Time last_time = ros::Time::now();
KDL::Frame r_HAND_target;
int loop_counter = 10000;
ros::Rate loop_rate(100);
while (true) {
generatePossiblePose(r_HAND_target, q, ndof, effector_name, col_model, kin_model);
sim->setState(q, dq, ddq);
sim->setTarget(r_HAND_target);
sim->oneStep();
if (!sim->inCollision()) {
break;
}
}
while (ros::ok()) {
if (loop_counter > 500) {
Eigen::VectorXd q_tmp(ndof);
generatePossiblePose(r_HAND_target, q_tmp, ndof, effector_name, col_model, kin_model);
sim->setTarget(r_HAND_target);
publishTransform(br, r_HAND_target, "effector_dest", "base");
loop_counter = 0;
}
loop_counter += 1;
sim->oneStep(&markers_pub_, 3000);
/* if (sim->inCollision() && !collision_in_prev_step) {
collision_in_prev_step = true;
std::cout << "collision begin" << std::endl;
}
else if (!sim->inCollision() && collision_in_prev_step) {
collision_in_prev_step = false;
std::cout << "collision end" << std::endl;
}
*/
sim->getState(q, dq, ddq);
// publish markers and robot state with limited rate
ros::Duration time_elapsed = ros::Time::now() - last_time;
if (time_elapsed.toSec() > 0.05) {
// calculate forward kinematics for all links
for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) {
kin_model->calculateFk(links_fk[l_idx], col_model->getLinkName(l_idx), q);
}
publishJointState(joint_state_pub_, q, joint_names);
int m_id = 0;
m_id = addRobotModelVis(markers_pub_, m_id, col_model, links_fk);
markers_pub_.publish();
ros::Time last_time = ros::Time::now();
}
ros::spinOnce();
loop_rate.sleep();
}
}
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = scan.header.frame_id;
ident.stamp_ = scan.header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
#ifdef LOGTOFILE
writeLaserPoseStamped(laser_pose);
#endif
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
double yaw = tf::getYaw(laser_pose.getRotation());
GMapping::OrientedPoint gmap_pose(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
ROS_DEBUG("laser's pose wrt base: %.3f %.3f %.3f",
laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, scan.angle_min);
tf::Stamped<tf::Quaternion> min_q(q, scan.header.stamp,
scan.header.frame_id);
q.setRPY(0.0, 0.0, scan.angle_max);
tf::Stamped<tf::Quaternion> max_q(q, scan.header.stamp,
scan.header.frame_id);
try
{
tf_.transformQuaternion(base_frame_, min_q, min_q);
tf_.transformQuaternion(base_frame_, max_q, max_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return false;
}
#ifdef LOGTOFILE
writeMinMax(min_q);
writeMinMax(max_q);
#endif
gsp_laser_beam_count_ = scan.ranges.size();
double angle_min = tf::getYaw(min_q);
double angle_max = tf::getYaw(max_q);
gsp_laser_angle_increment_ = scan.angle_increment;
ROS_DEBUG("Laser angles in base frame: min: %.3f max: %.3f inc: %.3f", angle_min, angle_max, gsp_laser_angle_increment_);
// setting maxRange and maxUrange here so we can set a reasonable default
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(gsp_laser_angle_increment_),
gmap_pose,
0.0,
maxRange_);
ROS_ASSERT(gsp_laser_);
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);
/// @todo Expose setting an initial pose
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,time(NULL));
ROS_INFO("Initialization complete");
return true;
}
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
{
// Check whether we know about this laser yet
if(lasers_.find(scan->header.frame_id) == lasers_.end())
{
// New laser; need to create a Karto device for it.
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = scan->header.frame_id;
ident.stamp_ = scan->header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return NULL;
}
double yaw = tf::getYaw(laser_pose.getRotation());
ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
scan->header.frame_id.c_str(),
laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, scan->header.stamp,
scan->header.frame_id);
q.setRPY(0.0, 0.0, scan->angle_max);
tf::Stamped<tf::Quaternion> max_q(q, scan->header.stamp,
scan->header.frame_id);
try
{
tf_.transformQuaternion(base_frame_, min_q, min_q);
tf_.transformQuaternion(base_frame_, max_q, max_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return false;
}
double angle_min = tf::getYaw(min_q);
double angle_max = tf::getYaw(max_q);
bool inverse = lasers_inverted_[scan->header.frame_id] = angle_max < angle_min;
if (inverse)
ROS_INFO("laser is mounted upside-down");
// Create a laser range finder device and copy in data from the first
// scan
std::string name = scan->header.frame_id;
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw));
laser->SetMinimumRange(scan->range_min);
laser->SetMaximumRange(scan->range_max);
laser->SetMinimumAngle(scan->angle_min);
laser->SetMaximumAngle(scan->angle_max);
laser->SetAngularResolution(scan->angle_increment);
// TODO: expose this, and many other parameters
//laser_->SetRangeThreshold(12.0);
// Store this laser device for later
lasers_[scan->header.frame_id] = laser;
// Add it to the dataset, which seems to be necessary
dataset_->Add(laser);
}
return lasers_[scan->header.frame_id];
}
