Float64List Canvas::getTotalMatrix()
{
// Maybe we should throw an exception instead of returning an empty matrix?
SkMatrix sk_matrix;
if (m_canvas)
sk_matrix = m_canvas->getTotalMatrix();
return toMatrix4(sk_matrix);
}
// 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();
}
}
