tf::Pose MotionModel::updateAction(tf::Pose& p)
{
double delta_rot1;
if (dxy < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
double delta_trans = dxy;
double delta_rot2 = angle_diff(delta_yaw, delta_rot1);
double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));
double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise +
alpha2 * delta_trans*delta_trans));
double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
alpha4 * delta_rot1_noise*delta_rot1_noise +
alpha4 * delta_rot2_noise*delta_rot2_noise);
double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
alpha2 * delta_trans*delta_trans));
delta_x = delta_trans_hat * cos(delta_rot1_hat);
delta_y = delta_trans_hat * sin(delta_rot1_hat);
delta_yaw = delta_rot1_hat + delta_rot2_hat;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
}
int CRvizMarker::Send(tf::Pose p, std::string frame) {
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = frame;
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = "nist_fanuc";
marker.id = _id++;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;
// Set the marker action. Options are ADD and DELETE
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
#if 0
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
#endif
marker.pose.position.x = p.getOrigin().x();
marker.pose.position.y = p.getOrigin().y();
marker.pose.position.z = p.getOrigin().z();
marker.pose.orientation.x = p.getRotation().x();
marker.pose.orientation.y = p.getRotation().y();
marker.pose.orientation.z = p.getRotation().z();
marker.pose.orientation.w = p.getRotation().w();
// Set the scale of the marker -- 1x1x1 here means 1m on a side!!!
marker.scale.x = scalex;
marker.scale.y = scaley;
marker.scale.z = scalez;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = r;
marker.color.g = g;
marker.color.b = b;
marker.color.a = a;
marker.lifetime = ros::Duration(0.0);
// Publish the marker
marker_pub.publish(marker);
ros::spinOnce();
ros::spinOnce();
return 0;
}
// Go down until hit the table. For safety min_height is specified. If no table found until this height, returns false.
// vertical_speed with which to move downwards
// thr_force - normal force threshold at which table is assumed to be detected
bool find_table_for_rolling(double min_height, double vertical_speed, double thr_force) {
double rate = 200;
thr_force = fabs(thr_force);
ros::Rate thread_rate(rate);
double startz = ee_pose.getOrigin().z();
msg_pose.pose.position.x = ee_pose.getOrigin().x();
msg_pose.pose.position.y = ee_pose.getOrigin().y();
msg_pose.pose.position.z = startz;
msg_pose.pose.orientation.x = ee_pose.getRotation().x();
msg_pose.pose.orientation.y = ee_pose.getRotation().y();
msg_pose.pose.orientation.z = ee_pose.getRotation().z();
msg_pose.pose.orientation.w = ee_pose.getRotation().w();
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
static tf::TransformBroadcaster br;
tf::Transform table;
table.setOrigin(tf::Vector3 (ee_pose.getOrigin().x(),ee_pose.getOrigin().y(),ee_pose.getOrigin().z() - min_height));
table.setRotation(tf::Quaternion (ee_pose.getRotation().x(),ee_pose.getRotation().y(),ee_pose.getRotation().z(),ee_pose.getRotation().w()));
br.sendTransform(tf::StampedTransform(table, ros::Time::now(), world_frame, "/attractor"));
}
ROS_INFO_STREAM("Finding table up to max dist. "<<min_height<<" with vertical speed "<<vertical_speed<<" and threshold force "<<thr_force<<"N.");
while(ros::ok()) {
msg_pose.pose.position.z = msg_pose.pose.position.z - vertical_speed/rate;
pub_.publish(msg_pose);
// Go down until force reaches the threshold
if(fabs(ee_ft[2]) > thr_force) {
break;
}
if(fabs(ee_pose.getOrigin().z()-startz) > min_height) {
ROS_INFO("Max distance reached");
return false;
}
thread_rate.sleep();
feedback_.progress = ee_ft[2];
as_.publishFeedback(feedback_);
}
if(!ros::ok()) {
return false;
}
tf::Vector3 table(ee_pose.getOrigin());
ROS_INFO_STREAM("Table found at height "<<table[2]);
msg_pose.pose.position.z = table[2];
pub_.publish(msg_pose);
sendAndWaitForNormalForce(0);
return true;
}
geometry_msgs::Pose tfTransformToGeometryPose(const tf::Pose& goal_pose)
{
geometry_msgs::Pose target_pose1;
target_pose1.orientation.x = goal_pose.getRotation().getX();
target_pose1.orientation.y = goal_pose.getRotation().getY();
target_pose1.orientation.z = goal_pose.getRotation().getZ();
target_pose1.orientation.w = goal_pose.getRotation().getW();
target_pose1.position.x = goal_pose.getOrigin().getX(); // + std::sin(angle)*radius;
target_pose1.position.y = goal_pose.getOrigin().getY(); // + std::cos(angle)*radius;
target_pose1.position.z = goal_pose.getOrigin().getZ();
return target_pose1;
}
void MotionModel::setMotion(const tf::Pose& pnew, const tf::Pose& pold)
{
tf::Pose delta_pose;
tf::Transform odom_to_base(pold.inverse().getRotation());
delta_pose.setOrigin(odom_to_base * (pnew.getOrigin() - pold.getOrigin()));
delta_pose.setRotation(pnew.getRotation() * pold.getRotation().inverse());
delta_x = delta_pose.getOrigin().x();
delta_y = delta_pose.getOrigin().y();
delta_yaw = atan2(sin(tf::getYaw(delta_pose.getRotation())), cos(tf::getYaw(delta_pose.getRotation())));
dxy=sqrt(delta_x*delta_x+delta_y*delta_y);
}
// The famous sendPose!
void sendPose(const tf::Pose& pose_) {
geometry_msgs::PoseStamped msg;
msg.pose.position.x = pose_.getOrigin().x();
msg.pose.position.y = pose_.getOrigin().y();
msg.pose.position.z = pose_.getOrigin().z();
msg.pose.orientation.x = pose_.getRotation().x();
msg.pose.orientation.y = pose_.getRotation().y();
msg.pose.orientation.z = pose_.getRotation().z();
msg.pose.orientation.w = pose_.getRotation().w();
pub_.publish(msg);
}
geometry_msgs::Pose tfPoseToGeometryPose(const tf::Pose tPose)
{
geometry_msgs::Pose gPose;
gPose.position.x = tPose.getOrigin().x();
gPose.position.y = tPose.getOrigin().y();
gPose.position.z = tPose.getOrigin().z();
gPose.orientation.x = tPose.getRotation().x();
gPose.orientation.y = tPose.getRotation().y();
gPose.orientation.z = tPose.getRotation().z();
gPose.orientation.w = tPose.getRotation().w();
return gPose;
}
/**
* Convert tf::Pose to string
*/
template<> std::string toString<tf::Pose>(const tf::Pose& p_pose)
{
std::stringstream ss;
ss << "Pose(Quaternion=" << toString(p_pose.getRotation()) << "; Vector3=" << toString(p_pose.getOrigin()) << ")";
return ss.str();
}
tf::Pose MotionModel::drawFromMotion(tf::Pose& p)
{
double sxy=0.3*srr;
delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
delta_yaw=fmod(delta_yaw, 2*M_PI);
if (delta_yaw>M_PI)
delta_yaw-=2*M_PI;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
return p;
}
geometry_msgs::Twist PoseFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
{
geometry_msgs::Twist res;
tf::Pose diff = pose2.inverse() * pose1;
res.linear.x = diff.getOrigin().x();
res.linear.y = diff.getOrigin().y();
res.angular.z = tf::getYaw(diff.getRotation());
if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
return res;
//in the case that we're not rotating to our goal position and we have a non-holonomic robot
//we'll need to command a rotational velocity that will help us reach our desired heading
//we want to compute a goal based on the heading difference between our pose and the target
double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));
//we'll also check if we can move more effectively backwards
if (allow_backwards_)
{
double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));
//check if its faster to just back up
if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
yaw_diff = neg_yaw_diff;
}
}
//compute the desired quaterion
tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
tf::Quaternion rot = pose2.getRotation() * rot_diff;
tf::Pose new_pose = pose1;
new_pose.setRotation(rot);
diff = pose2.inverse() * new_pose;
res.linear.x = diff.getOrigin().x();
res.linear.y = diff.getOrigin().y();
res.angular.z = tf::getYaw(diff.getRotation());
return res;
}
MathLib::Matrix4 toMatrix4(const tf::Pose& pose) {
MathLib::Matrix4 mat;
mat.Identity();
tf::Matrix3x3 mat33(pose.getRotation());
mat.SetTranslation(MathLib::Vector3(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()));
mat.SetOrientation(MathLib::Matrix3(mat33[0][0], mat33[0][1], mat33[0][2],
mat33[1][0], mat33[1][1], mat33[1][2],
mat33[2][0], mat33[2][1], mat33[2][2]));
return mat;
}
// Go to this pose
bool go_home(tf::Pose& pose_) {
tf::Vector3 start_p(pose_.getOrigin());
tf::Quaternion start_o(pose_.getRotation());
msg_pose.pose.position.x = start_p.x();
msg_pose.pose.position.y = start_p.y();
msg_pose.pose.position.z = start_p.z();
msg_pose.pose.orientation.w = start_o.w();
msg_pose.pose.orientation.x = start_o.x();
msg_pose.pose.orientation.y = start_o.y();
msg_pose.pose.orientation.z = start_o.z();
pub_.publish(msg_pose);
sendNormalForce(0);
ros::Rate thread_rate(2);
ROS_INFO("Homing...");
while(ros::ok()) {
double oerr =(ee_pose.getRotation() - start_o).length() ;
double perr = (ee_pose.getOrigin() - start_p).length();
feedback_.progress = 0.5*(perr+oerr);
as_.publishFeedback(feedback_);
ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
if(perr< 0.02 && oerr < 0.02) {
break;
}
if (as_.isPreemptRequested() || !ros::ok() )
{
sendNormalForce(0);
sendPose(ee_pose);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
thread_rate.sleep();
}
return ros::ok();
}
// Convert from tf::Pose to CameraPose (position and orientation)
inline void TFPose2CameraPose(const tf::Pose& pose, CameraPose& cameraPose)
{
// convert to position opencv vector
tf::Vector3 position_tf = pose.getOrigin();
cv::Point3d position = cv::Point3d(position_tf.getX(), position_tf.getY(), position_tf.getZ());
// Convert to orientation opencv quaternion
tf::Quaternion orientation_tf = pose.getRotation();
cv::Vec4d orientation(orientation_tf.getW(), orientation_tf.getX(), orientation_tf.getY(), orientation_tf.getZ());
cameraPose = CameraPose(position, orientation);
}
void toMsgPose(const tf::Pose& tf_pose, geometry_msgs::Pose& msg_pose)
{
msg_pose.position.x = tf_pose.getOrigin().getX();
msg_pose.position.y = tf_pose.getOrigin().getY();
msg_pose.position.z = tf_pose.getOrigin().getZ();
tf::Quaternion orientation = tf_pose.getRotation();
msg_pose.orientation.w = orientation.getW();
msg_pose.orientation.x = orientation.getX();
msg_pose.orientation.y = orientation.getY();
msg_pose.orientation.z = orientation.getZ();
}
// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
ROS_INFO_STREAM("Received Position Command");
const geometry_msgs::PoseStamped* data = msg.get();
des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));
ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());
if(bOrientCtrl) {
tf::Quaternion q = des_ee_pose.getRotation();
ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
}
}
static RVector<double> PoseValues(tf::Pose & pose) {
RVector<double> values;
values.push_back( pose.getOrigin().x());
values.push_back( pose.getOrigin().y());
values.push_back( pose.getOrigin().z());
double roll, pitch, yaw;
tf::Quaternion q = pose.getRotation();
tf::Matrix3x3(q).getRPY(roll, pitch, yaw );
values.push_back( roll );
values.push_back( pitch );
values.push_back( yaw );
return values;
}
void sendPose(tf::Pose& pose) {
msg_pose.header.stamp = ros::Time::now();
tf::Vector3 tmp = pose.getOrigin();
msg_pose.pose.position.x = tmp.x();
msg_pose.pose.position.y = tmp.y();
msg_pose.pose.position.z = tmp.z();
tf::Quaternion quat = pose.getRotation();
msg_pose.pose.orientation.w = quat.w();
msg_pose.pose.orientation.x = quat.x();
msg_pose.pose.orientation.y = quat.y();
msg_pose.pose.orientation.z = quat.z();
pub_pose.publish(msg_pose);
}
// Roll with "force" and horizontal "speed" until the length "range"
bool rolling(double force, double speed, double range) {
ROS_INFO_STREAM("Rolling with force "<<force<<", speed "<<speed<<", range "<<range);
force = fabs(force);
sendNormalForce(-force);
msg_pose.pose.position.x = ee_pose.getOrigin().x();
msg_pose.pose.position.y = ee_pose.getOrigin().y();
msg_pose.pose.position.z = ee_pose.getOrigin().z();
tf::Quaternion q = ee_pose.getRotation();
msg_pose.pose.orientation.x = q.x();
msg_pose.pose.orientation.y = q.y();
msg_pose.pose.orientation.z = q.z();
msg_pose.pose.orientation.w = q.w();
double center = ee_pose.getOrigin().y();
double rate = 200;
ros::Rate thread_rate(rate);
int count=0;
while(ros::ok()) {
msg_pose.pose.position.y = msg_pose.pose.position.y + speed/rate;
feedback_.progress = msg_pose.pose.position.y;
as_.publishFeedback(feedback_);
if( fabs(msg_pose.pose.position.y - center) > range) {
ROS_INFO("Change direction");
speed *= -1;
if(++count > 5) {
break;
}
}
pub_.publish(msg_pose);
thread_rate.sleep();
}
msg_pose.pose.position.z = ee_pose.getOrigin().z() + 0.15;
pub_.publish(msg_pose);
sendNormalForce(0);
return true;
}
void NaoqiJointStates::run()
{
ros::Rate r(m_rate);
ros::Time stamp1;
ros::Time stamp2;
ros::Time stamp;
std::vector<float> odomData;
float odomX, odomY, odomZ, odomWX, odomWY, odomWZ;
std::vector<float> memData;
std::vector<float> positionData;
ROS_INFO("Staring main loop. ros::ok() is %d nh.ok() is %d",ros::ok(),m_nh.ok());
while(ros::ok() )
{
r.sleep();
ros::spinOnce();
stamp1 = ros::Time::now();
try
{
memData = m_memoryProxy->getListData(m_dataNamesList);
// {SPACE_TORSO = 0, SPACE_WORLD = 1, SPACE_NAO = 2}. (second argument)
odomData = m_motionProxy->getPosition("Torso", 1, true);
positionData = m_motionProxy->getAngles("Body", true);
}
catch (const AL::ALError & e)
{
ROS_ERROR( "Error accessing ALMemory, exiting...");
ROS_ERROR( "%s", e.what() );
//ros.signal_shutdown("No NaoQI available anymore");
}
stamp2 = ros::Time::now();
//ROS_DEBUG("dt is %f",(stamp2-stamp1).toSec()); % dt is typically around 1/1000 sec
// TODO: Something smarter than this..
stamp = stamp1 + ros::Duration((stamp2-stamp1).toSec()/2.0);
/******************************************************************
* IMU
*****************************************************************/
if (memData.size() != m_dataNamesList.getSize())
{
ROS_ERROR("memData length %zu does not match expected length %u",memData.size(),m_dataNamesList.getSize() );
continue;
}
// IMU data:
m_torsoIMU.header.stamp = stamp;
float angleX = memData[1];
float angleY = memData[2];
float angleZ = memData[3];
float gyroX = memData[4];
float gyroY = memData[5];
float gyroZ = memData[6];
float accX = memData[7];
float accY = memData[8];
float accZ = memData[9];
m_torsoIMU.orientation = tf::createQuaternionMsgFromRollPitchYaw(
angleX,
angleY,
angleZ); // yaw currently always 0
m_torsoIMU.angular_velocity.x = gyroX;
m_torsoIMU.angular_velocity.y = gyroY;
m_torsoIMU.angular_velocity.z = gyroZ; // currently always 0
m_torsoIMU.linear_acceleration.x = accX;
m_torsoIMU.linear_acceleration.y = accY;
m_torsoIMU.linear_acceleration.z = accZ;
// covariances unknown
// cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
m_torsoIMU.orientation_covariance[0] = -1;
m_torsoIMU.angular_velocity_covariance[0] = -1;
m_torsoIMU.linear_acceleration_covariance[0] = -1;
m_torsoIMUPub.publish(m_torsoIMU);
/******************************************************************
* Joint state
*****************************************************************/
m_jointState.header.stamp = stamp;
m_jointState.header.frame_id = m_baseFrameId;
m_jointState.position.resize(positionData.size());
for(unsigned i = 0; i<positionData.size(); ++i)
{
m_jointState.position[i] = positionData[i];
}
// simulated model misses some joints, we need to fill:
if (m_jointState.position.size() == 22)
{
m_jointState.position.insert(m_jointState.position.begin()+6,0.0);
m_jointState.position.insert(m_jointState.position.begin()+7,0.0);
m_jointState.position.push_back(0.0);
m_jointState.position.push_back(0.0);
}
m_jointStatePub.publish(m_jointState);
/******************************************************************
* Odometry
*****************************************************************/
if (!m_paused) {
// apply offset transformation:
tf::Pose transformedPose;
if (odomData.size()!=6)
{
ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() );
continue;
}
double dt = (stamp.toSec() - m_lastOdomTime);
odomX = odomData[0];
odomY = odomData[1];
odomZ = odomData[2];
odomWX = odomData[3];
odomWY = odomData[4];
odomWZ = odomData[5];
tf::Quaternion q;
// roll and pitch from IMU, yaw from odometry:
if (m_useIMUAngles)
q.setRPY(angleX, angleY, odomWZ);
else
q.setRPY(odomWX, odomWY, odomWZ);
m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ));
m_odomPose.setRotation(q);
if(m_mustUpdateOffset) {
if(!m_isInitialized) {
if(m_initializeFromIMU) {
// Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ));
} else if(m_initializeFromOdometry) {
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ));
}
m_isInitialized = true;
} else {
// Overwrite target pitch and roll angles with IMU data
const double target_yaw = tf::getYaw(m_targetPose.getRotation());
if(m_initializeFromIMU) {
m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw));
} else if(m_initializeFromOdometry){
m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw));
}
}
m_odomOffset = m_targetPose * m_odomPose.inverse();
transformedPose = m_targetPose;
m_mustUpdateOffset = false;
} else {
transformedPose = m_odomOffset * m_odomPose;
}
//
// publish the transform over tf first
//
m_odomTransformMsg.header.stamp = stamp;
tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
m_transformBroadcaster.sendTransform(m_odomTransformMsg);
//
// Fill the Odometry msg
//
m_odom.header.stamp = stamp;
//set the velocity first (old values still valid)
m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt;
m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt;
m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt;
// TODO: calc angular velocity!
// m_odom.twist.twist.angular.z = vth; ??
//set the position from the above calculated transform
m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
m_odomPub.publish(m_odom);
m_lastOdomTime = stamp.toSec();
}
}
ROS_INFO("naoqi_sensors stopped.");
}
// Do stuff with learned models
// Phase - Reach, Roll or Back?
// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
// reachingThreshold - you know
// model_dt - you know
bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {
ROS_INFO_STREAM(" Model Path "<<model_base_path);
ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
ROS_INFO_STREAM(" Model DT "<<model_dt);
if (force_gmm_id!="")
ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);
ros::Rate wait(1);
tf::Transform trans_final_target, ee_pos_att;
// To Visualize EE Frames
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));
// convert attractor information to world frame
trans_final_target.mult(trans_obj, trans_att);
ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
}
// Initialize CDS
CDSExecution *cdsRun = new CDSExecution;
cdsRun->initSimple(model_base_path, phase, force_gmm_id);
cdsRun->setObjectFrame(toMatrix4(trans_obj));
cdsRun->setAttractorFrame(toMatrix4(trans_att));
cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
cdsRun->setDT(model_dt);
if (phase==PHASEBACK || phase==PHASEROLL)
masterType = CDSController::LINEAR_DYNAMICS;
// Roll slow but move fast for reaching and back phases.
// If models have proper speed, this whole block can go!
if(phase == PHASEROLL) {
//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
// large threshold to avoid blocking forever
// TODO: should rely on preempt in action client.
// reachingThreshold = 0.02;
reachingThreshold = 0.025;
} else {
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
}
cdsRun->postInit();
// If phase is rolling, need force model as well
GMR* gmr_perr_force = NULL;
if(phase == PHASEROLL) {
gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
if(!gmr_perr_force) {
ROS_ERROR("Cannot initialize GMR model");
return false;
}
}
ros::Duration loop_rate(model_dt);
tf::Pose mNextRobotEEPose = ee_pose;
std::vector<double> gmr_in, gmr_out;
gmr_in.resize(1);gmr_out.resize(1);
double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
tf::Vector3 att_t, curr_t;
ROS_INFO("Execution started");
tf::Pose p;
bool bfirst = true;
std_msgs::String string_msg, action_name_msg, model_fname_msg;
std::stringstream ss, ss_model;
if(phase == PHASEREACH) {
ss << "reach ";
}
else if (phase == PHASEROLL){
ss << "roll";
}
else if (phase == PHASEBACK){
ss << "back";
}
if (!homing){
string_msg.data = ss.str();
pub_action_state_.publish(string_msg);
// ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
if (force_gmm_id=="")
ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
else
ss_model << path_matlab_plot << "/Phase" << phase << "/ForceProfile_" << force_gmm_id << ".fig";
//ss_model << "/Phase" << phase << "/masterGMM.fig";
model_fname_msg.data = ss_model.str();
pub_model_fname_.publish(model_fname_msg);
action_name_msg.data = ss.str();
pub_action_name_.publish(action_name_msg);
plot_dyn = 1;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
}
while(ros::ok()) {
if (!homing)
plot_dyn = 1;
else
plot_dyn = 0;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
// Nadia's stuff
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);
/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/
switch (phase) {
// Home, reach and back are the same control-wise
case PHASEREACH:
case PHASEBACK:
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());
// Compute Next Desired EE Pose
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
// going the wrong way around
p.setRotation(trans_final_target.getRotation());
//Publish desired force
gmr_msg.data = 0.0;
pub_gmr_out_.publish(gmr_msg);
break;
case PHASEROLL:
// Current progress in rolling phase is simply the position error
prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
// New position error being fed to GMR Force Model
att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
new_err = (att_t - curr_t).length();
gmr_err = new_err;
//Hack! Truncate errors to corresponding models
if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
gmr_err = 0.03;
}
if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
gmr_err = 0.07;
}
if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
gmr_err = 0.13;
}
//pos_err = prog_curr;
ori_err = 0;
gmr_err = gmr_err;
gmr_in[0] = gmr_err; // distance between EE and attractor
// Query the model for desired force
getGMRResult(gmr_perr_force, gmr_in, gmr_out);
/* double fz_plot;
getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
// Send fz and distance to attractor for plotting
msg_ft.wrench.force.x = gmr_err;
msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
msg_ft.wrench.force.z = 0;
msg_ft.wrench.torque.x = 0;
msg_ft.wrench.torque.y = 0;
msg_ft.wrench.torque.z = 0;
pub_ee_ft_att_.publish(msg_ft);
// Hack! Scale the force to be in reasonable values
gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);
ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);
gmr_msg.data = gmr_out[0];
pub_gmr_out_.publish(gmr_msg);
// Hack! Safety first!
if(gmr_out[0] > MAX_ROLLING_FORCE) {
gmr_out[0] = MAX_ROLLING_FORCE;
}
// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
if(bfirst) {
sendAndWaitForNormalForce(-gmr_out[0]);
bfirst = false;
} else {
sendNormalForce(-gmr_out[0]);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Hack! Dont rely on model orientation. Use target orientation instead.
p.setRotation(trans_final_target.getRotation());
// Hack! Dont rely on the Z component of the model. It might go below the table!
p.getOrigin().setZ(trans_final_target.getOrigin().getZ());
homing=false;
break;
default:
ROS_ERROR_STREAM("No such phase defined "<<phase);
return false;
}
// Add rotation of Tool wrt. flange_link for BOXY
/*if (use_boxy_tool){
tf::Matrix3x3 R = p.getBasis();
Eigen::Matrix3d R_ee;
tf::matrixTFToEigen(R,R_ee);
Eigen::Matrix3d R_tool;
R_tool << -0.7071, -0.7071, 0.0,
0.7071,-0.7071, 0.0,
0.0, 0.0, 1.0;
//multiply tool rot
R_ee = R_ee*R_tool;
tf::matrixEigenToTF(R_ee,R);
p.setBasis(R);
}*/
// Send the computed pose from one of the above phases
sendPose(p);
// convert and send ee pose to attractor frame to plots
ee_pos_att.mult(trans_final_target.inverse(), p);
geometry_msgs::PoseStamped msg;
msg.pose.position.x = ee_pos_att.getOrigin().x();
msg.pose.position.y = ee_pos_att.getOrigin().y();
msg.pose.position.z = ee_pos_att.getOrigin().z();
msg.pose.orientation.x = ee_pos_att.getRotation().x();
msg.pose.orientation.y = ee_pos_att.getRotation().y();
msg.pose.orientation.z = ee_pos_att.getRotation().z();
msg.pose.orientation.w = ee_pos_att.getRotation().w();
pub_ee_pos_att_.publish(msg);
//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
sendPose(ee_pose);
sendNormalForce(0);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
feedback_.progress = prog_curr;
as_.publishFeedback(feedback_);
/* if(prog_curr < reachingThreshold) {
break;
}*/
//Orientation error 0.05
if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {
break;
}
loop_rate.sleep();
}
delete cdsRun;
if(phase == PHASEREACH) {
// Hack! If phase is "reach", find the table right after reaching
if (bWaitForForces && !homing) {
bool x = find_table_for_rolling(0.35, 0.05, 5);
// bool x = find_table_for_rolling(0.35, 0.1, 5);
//-> Send command to dynamics plotter to stop logging
return x;
}
if (!homing){
//-> Send command to dynamics plotter to stop logging
}
} else if (phase == PHASEROLL){
// Hack! wait for zero force before getting ready to recieve further commands.
// This is to avoid dragging the dough.
sendAndWaitForNormalForce(0);
//-> Send command to dynamics plotter to start plotting
return ros::ok();
} else {
//->Send command to dynamics plotter to start plotting
return ros::ok();
}
}
bool
FootstepNavigation::getFootstep(const tf::Pose& from,
const State& from_planned,
const State& to,
humanoid_nav_msgs::StepTarget* footstep)
{
// get footstep to reach 'to' from 'from'
tf::Transform step = from.inverse() *
tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
tf::Point(to.getX(), to.getY(), 0.0));
// set the footstep
footstep->pose.x = step.getOrigin().x();
footstep->pose.y = step.getOrigin().y();
footstep->pose.theta = tf::getYaw(step.getRotation());
if (to.getLeg() == LEFT)
footstep->leg = humanoid_nav_msgs::StepTarget::left;
else // to.leg == RIGHT
footstep->leg = humanoid_nav_msgs::StepTarget::right;
/* check if the footstep can be performed by the NAO robot ******************/
// check if the step lies within the executable range
if (performable(*footstep))
{
return true;
}
else
{
// check if there is only a minor divergence between the current support
// foot and the foot placement used during the plannig task: in such a case
// perform the step that has been used during the planning
float step_diff_x = fabs(from.getOrigin().x() - from_planned.getX());
float step_diff_y = fabs(from.getOrigin().y() - from_planned.getY());
float step_diff_theta = fabs(
angles::shortest_angular_distance(
tf::getYaw(from.getRotation()), from_planned.getTheta()));
if (step_diff_x < ivAccuracyX && step_diff_y < ivAccuracyY &&
step_diff_theta < ivAccuracyTheta)
{
step = tf::Pose(tf::createQuaternionFromYaw(from_planned.getTheta()),
tf::Point(from_planned.getX(), from_planned.getY(), 0.0)
).inverse() *
tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
tf::Point(to.getX(), to.getY(), 0.0));
footstep->pose.x = step.getOrigin().x();
footstep->pose.y = step.getOrigin().y();
footstep->pose.theta = tf::getYaw(step.getRotation());
return true;
}
return false;
}
// // ALTERNATIVE: clip the footstep into the executable range; if nothing was
// // clipped: perform; if too much was clipped: do not perform
// humanoid_nav_msgs::ClipFootstep footstep_clip;
// footstep_clip.request.step = footstep;
// ivClipFootstepSrv.call(footstep_clip);
//
// if (performanceValid(footstep_clip))
// {
// footstep.pose.x = footstep_clip.response.step.pose.x;
// footstep.pose.y = footstep_clip.response.step.pose.y;
// footstep.pose.theta = footstep_clip.response.step.pose.theta;
// return true;
// }
// else
// {
// return false;
// }
}
void OdometryRemap::torsoOdomCallback(const nao_msgs::TorsoOdometryConstPtr& msgOdom, const nao_msgs::TorsoIMUConstPtr& msgIMU){
if (m_paused){
ROS_DEBUG("Skipping odometry callback, paused");
return;
}
double odomTime = (msgOdom->header.stamp).toSec();
ROS_DEBUG("Received [%f %f %f %f (%f/%f) (%f/%f) %f]", odomTime, msgOdom->x, msgOdom->y, msgOdom->z, msgOdom->wx, msgIMU->angleX, msgOdom->wy, msgIMU->angleY, msgOdom->wz);
double dt = (odomTime - m_lastOdomTime);
tf::Quaternion q;
// roll and pitch from IMU, yaw from odometry:
if (m_useIMUAngles)
q.setRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz);
else
q.setRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz);
m_odomPose.setOrigin(tf::Vector3(msgOdom->x, msgOdom->y, msgOdom->z));
m_odomPose.setRotation(q);
// apply offset transformation:
tf::Pose transformedPose;
if(m_mustUpdateOffset) {
if(!m_isInitialized) {
if(m_initializeFromIMU) {
// Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz));
} else if(m_initializeFromOdometry) {
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz));
}
m_isInitialized = true;
} else {
// Overwrite target pitch and roll angles with IMU data
const double target_yaw = tf::getYaw(m_targetPose.getRotation());
if(m_initializeFromIMU) {
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, target_yaw));
} else if(m_initializeFromOdometry){
m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, target_yaw));
}
}
m_odomOffset = m_targetPose * m_odomPose.inverse();
transformedPose = m_targetPose;
m_mustUpdateOffset = false;
} else {
transformedPose = m_odomOffset * m_odomPose;
}
// publish the transform over tf first:
m_odomTransformMsg.header.stamp = msgOdom->header.stamp;
tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
m_transformBroadcaster.sendTransform(m_odomTransformMsg);
//next, publish the actual odometry message:
m_odom.header.stamp = msgOdom->header.stamp;
m_odom.header.frame_id = m_odomFrameId;
//set the velocity first (old values still valid)
m_odom.twist.twist.linear.x = (msgOdom->x - m_odom.pose.pose.position.x) / dt;
m_odom.twist.twist.linear.y = (msgOdom->y - m_odom.pose.pose.position.y) / dt;
m_odom.twist.twist.linear.z = (msgOdom->z - m_odom.pose.pose.position.z) / dt;
// TODO: calc angular velocity!
// m_odom.twist.twist.angular.z = vth; ??
//set the position from the above calculated transform
m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
//publish the message
m_odomPub.publish(m_odom);
m_lastOdomTime = odomTime;
// TODO: not used
// m_lastWx = msgOdom->wx;
// m_lastWy = msgOdom->wy;
// m_lastWz = msgOdom->wz;
}
// Convert current cartesian commands to joint velocity
void computeJointVelocity(Eigen::VectorXd& jvel) {
if(jvel.size() != numdof) {
jvel.resize(numdof);
}
if(est_ee_ft.norm() > max_ee_ft_norm) {
for(int i=0; i<jvel.size(); ++i) {
jvel[i]=0.0;
}
ROS_WARN_THROTTLE(0.1, "Too much force! Sending zero velocity");
return;
}
// tf::Pose curr_ee_pose;
mRobot->setJoints(read_jpos);
mRobot->getEEPose(curr_ee_pose);
// Two kinds of cartesian velocities - due to position error and force error
Eigen::VectorXd vel_due_to_pos, vel_due_to_force;
if(bOrientCtrl) {
vel_due_to_pos.resize(6);
vel_due_to_force.resize(6);
} else {
vel_due_to_pos.resize(3);
vel_due_to_force.resize(3);
}
for(int i=0; i<vel_due_to_force.size(); ++i) {
vel_due_to_force[i] = 0;
vel_due_to_pos[i] = 0;
}
ROS_INFO_STREAM_THROTTLE(0.5, "Publishing EE TF");
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(des_ee_pose, ros::Time::now(), world_frame, "/des_ee_tf"));
br.sendTransform(tf::StampedTransform(curr_ee_pose, ros::Time::now(), world_frame, "/curr_ee_tf"));
// Cartesian velocity due to position/orientation error
tf::Vector3 linvel = des_ee_pose.getOrigin() - curr_ee_pose.getOrigin();
vel_due_to_pos(0) = linvel.getX();
vel_due_to_pos(1) = linvel.getY();
vel_due_to_pos(2) = linvel.getZ();
double pos_err = linvel.length();
ROS_INFO_STREAM_THROTTLE(0.5, "Position Err:\t"<< pos_err);
// Nadia's way...
// If Orientation is activated from launch file
double qdiff = acos(abs(des_ee_pose.getRotation().dot(curr_ee_pose.getRotation())));
if(bOrientCtrl) {
// Computing angular velocity using quaternion difference
tf::Quaternion tmp = curr_ee_pose.getRotation();
tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
vel_due_to_pos(3) = angvel.getX();
vel_due_to_pos(4) = angvel.getY();
vel_due_to_pos(5) = angvel.getZ();
ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<qdiff);
}
if (pos_err < 0.01 && qdiff < 0.05){ // LASA 0.5deg
// if (pos_err < 0.015 && qdiff < 0.05){ // Boxy 1.7 deg
vel_due_to_pos(3) = 0;
vel_due_to_pos(4) = 0;
vel_due_to_pos(5) = 0;
}
/*
// Aswhini's way...
// If Orientation is activated from launch file
if(bOrientCtrl) {
// Computing angular velocity using quaternion difference
tf::Quaternion tmp = curr_ee_pose.getRotation();
tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
vel_due_to_pos(3) = angvel.getX();
vel_due_to_pos(4) = angvel.getY();
vel_due_to_pos(5) = angvel.getZ();
tf::Quaternion t = angvel;
// ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<angvel.length());
}
*/
// Compute errors in position
double err = vel_due_to_pos.norm();
// Increase tracking performance by tuning traj_traking_gain
vel_due_to_pos *= traj_tracking_gain;
// If force is activated from launch file
if(bUseForce) {
// Compute force error
Eigen::VectorXd ft_err = des_ee_ft - est_ee_ft;
double ft_err_nrm;
if(bOrientCtrl) {
ft_err_nrm = ft_err.norm();
} else {
double force_error = ft_err[0]*ft_err[0] +ft_err[1]*ft_err[1]+ft_err[2]*ft_err[2];
ft_err_nrm = sqrt(force_error);
}
// Remove force if too small. This is necessary due to residual errors in EE force estimation.
// Dont apply external forces until desired position is almost reached within reach_tol
if(ft_err_nrm > ft_tol && err < reach_tol) {
forceErrorToVelocity(ft_err, vel_due_to_force);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force Err:\t"<<ft_err_nrm);
}
// Add the cartesian velocities due to position and force errors and compute the joint_velocity
// mRobot->getIKJointVelocity(vel_due_to_force+vel_due_to_pos, jvel);
mRobot->getIKJointVelocity(vel_due_to_pos, jvel);
}
