unlatching.cpp

Go to the documentation of this file.

#include "unlatching.h"
// arm movement
#include <ee_cart_imped_action/ee_cart_imped_arm.hh>
#include <cmath>  //for fabs
#include "switch_controller.h"


DoorOpener::DoorOpener(unsigned char side): 
    serviceServer(n.advertiseService("unlatch_door", &DoorOpener::listen, this)),
    pregrasp_sub(n, "pregrasp", 5),
    grasp_sub(n, "grasp", 5),
    postgrasp_sub(n, "postgrasp", 5),
    sync(MySyncPolicy(10),  pregrasp_sub, grasp_sub, postgrasp_sub),
    side_(side),
    tool_frame_name_("/r_gripper_tool_frame"),
    fixed_frame_name_("/torso_lift_link"),
    listening_(false),
    started_(false)
{
  cmd_vel_pub_= n.advertise<geometry_msgs::Twist>("/base_controller/command", 12);
  base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0;   

  tflistener_ = new tf::TransformListener; 
  
  std::string gac_name("/r_gripper_controller/gripper_action");
  if(side_ == 'l') gac_name[1] = side_;
  gripperClient = new gripActClient(gac_name.c_str(), true); 

  //EE-Cart-Imped Controller Interface for the desired arm
  std::string contr_name("/r_arm_cart_imped_controller");
  if(side_ == 'l') contr_name[1] = side_;
  std::cout << contr_name;
  arm_ = new EECartImpedArm(contr_name.c_str());

  if(side_ == 'l') tool_frame_name_[1] = side_;

}

bool DoorOpener::listen(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
  started_ = true;
  if(!listening_){
    success_ = false;
    listening_ = true;
    sync.registerCallback(boost::bind(&DoorOpener::grasp_handle, this, _1, _2, _3));
  }
  ROS_INFO("Waiting for unlatching to finish");
  while(ros::ok() && started_){
    ros::Duration(0.5).sleep();
    ROS_INFO_THROTTLE(30,"Still waiting for unlatching to finish");
  }
  return true;
}

DoorOpener::~DoorOpener(){
  delete gripperClient;
  delete arm_;
  delete tflistener_; 
}

void DoorOpener::use_gripper(float position, float effort) // Do not limit effort with negative value
{
    while(!gripperClient->waitForServer(ros::Duration(5.0)) && ros::ok())
        ROS_INFO("Should use gripper, but I am waiting for the gripper action server to come up");

    pr2_controllers_msgs::Pr2GripperCommandGoal goal;
    goal.command.position = position;
    goal.command.max_effort = effort;  

    gripperClient->sendGoal(goal);
}
bool DoorOpener::wait_for_gripper(){ // Do not limit effort with negative value
    gripperClient->waitForResult();

    if(gripperClient->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
        ROS_INFO("Successfully used gripper!");
        return true;
    } else {
        ROS_WARN("Failed to bring gripper to desired state"); 
        return false;
    }
}

/*
bool DoorOpener::use_gripper(float position, float effort) 
{
    //wait for the gripper action server to come up
    while(!gripperClient->waitForServer(ros::Duration(5.0)) && ros::ok()){
      ROS_INFO("Should use gripper, but I am waiting for the gripper action server to come up");
    } 
    pr2_controllers_msgs::Pr2GripperCommandGoal goal;
    goal.command.position = position;
    goal.command.max_effort = effort;  

    gripperClient->sendGoal(goal);
    gripperClient->waitForResult();

    if(gripperClient->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
        ROS_INFO("Successfully used gripper!");
        return true;
    } else {
        ROS_ERROR("Failed to bring gripper to position %f with effort %f!", position, effort); 
        return false;
    }
}
*/

void DoorOpener::drive_base(const ros::TimerEvent& event)
{
    if(base_cmd.linear.x == 0 && base_cmd.linear.y == 0 && base_cmd.angular.z == 0) return;
    //ROS_INFO("publishing base_cmd (x=%f, y=%f, z=%f)", base_cmd.angular.x, base_cmd.angular.y , base_cmd.angular.z  );
    cmd_vel_pub_.publish(base_cmd);
}


void DoorOpener::push_door()
{
    float base_speed = -0.05; //in cm/s

    // tf stuff
    tf::Transform gripper_transform, last_gripper_transform,
                  init_gripper_global_tf, odom_to_torso_tf ;

    //Get initial gripper position relative to torso
    last_gripper_transform = getTransform(fixed_frame_name_, tool_frame_name_);

    //Get initial gripper position relative to odometry
    init_gripper_global_tf = getTransform("/odom_combined", tool_frame_name_);

    float px,py = 0.0,pz,rx,ry,rz,rw;
    px = last_gripper_transform.getOrigin().getX(); //try to keep forward distance

    while(fabs(py) < 0.45){
        //wait for transformation of gripper
        gripper_transform = getTransform(fixed_frame_name_, tool_frame_name_);
        odom_to_torso_tf = getTransform("/odom_combined",fixed_frame_name_);
        tf::Transform first_gripper_pose = odom_to_torso_tf.inverseTimes(init_gripper_global_tf); // initial global gripper pose relative to current torso pose
        tf::Transform delta = first_gripper_pose.inverseTimes(gripper_transform);
        printTransform(first_gripper_pose, "gripper init pose in torso frame");
        printTransform(delta, "global delta in torso_lift");

        //Adapt position to passive motion
        py = gripper_transform.getOrigin().getY();
        pz = gripper_transform.getOrigin().getZ();
        rx = gripper_transform.getRotation().getX();
        ry = gripper_transform.getRotation().getY();
        rz = gripper_transform.getRotation().getZ();
        rw = gripper_transform.getRotation().getW();

        //try to go linearly further in the current direction 
        tf::Transform gripper_delta_tf = last_gripper_transform.inverseTimes(gripper_transform); 
        printTransform(gripper_delta_tf, "Linear Delta");
        py += gripper_delta_tf.getOrigin().getY()*23;
        if(gripper_delta_tf.getOrigin().getX() < 0.0001) ROS_WARN("Door not moving further");
        //no change in height desired: pz += gripper_delta_tf.getOrigin().getZ();

        ee_cart_imped_msgs::EECartImpedGoal pushDoor;//,traj_l;
        EECartImpedArm::addTrajectoryPoint(pushDoor, px,py,pz, rx, ry, rz, rw,
                                           1000, 550, 1000, //stiff forward and downward, loose to the sides. Rotatio
                                           30, 30, 1, //loose around vertical
                                           false, true, false, false, false,false,//stiff (not torqe)
                                           0.50,fixed_frame_name_);//cumulative time
        //Start Driving
        base_cmd.linear.x = base_speed;
        //Start Pushing
        arm_->startTrajectory(pushDoor);
        last_gripper_transform = gripper_transform;
        py = gripper_transform.getOrigin().getY();
    }                                        
    base_cmd.linear.x = 0.0;
    /* not used anyway
    ee_cart_imped_msgs::EECartImpedGoal pushDoor;//,traj_l;
    EECartImpedArm::addTrajectoryPoint(pushDoor, px,py,pz, rx, ry, rz, rw,
                                       100, 1000, 100, 30, 30, 30, //torques
                                       false, false, false, false, false,false,//stiff (not torqe)
                                       0.50,fixed_frame_name_);//cumulative time
                                       */
    ROS_INFO("Gripper is outside of robot footprint" );
}


void DoorOpener::home_position()
{
    ee_cart_imped_msgs::EECartImpedGoal homePos;//,traj_l;

    tf::Quaternion r(tf::Vector3(1,0,0), -M_PI/2.0);//gripper rotation
    float y = side_ == 'l'? 0.2 : -0.4;
    EECartImpedArm::addTrajectoryPoint(homePos, 0.35, y,-0.10, r.getX(), r.getY(), r.getZ(), r.getW(),
                                       100, 1000, 100, 3, 3, 3, //torques
                                       false, false, false, false, false,false,//stiff (not torqe)
                                       2,fixed_frame_name_);//cumulative time
    arm_->startTrajectory(homePos);
}


void DoorOpener::withdraw(float meters)
{
    tf::StampedTransform gripper_transform(getTransform(fixed_frame_name_, tool_frame_name_));
    tf::Vector3 gripperBackward(-meters, 0,0); 
    gripperBackward = gripper_transform * gripperBackward;

    float px,py,pz,rx,ry,rz,rw;
    px = gripperBackward.getX();
    py = gripperBackward.getY();
    pz = gripperBackward.getZ();

    rx = gripper_transform.getRotation().getX();
    ry = gripper_transform.getRotation().getY();
    rz = gripper_transform.getRotation().getZ();
    rw = gripper_transform.getRotation().getW();

    ee_cart_imped_msgs::EECartImpedGoal withdrawTraj;//,traj_l;
    EECartImpedArm::addTrajectoryPoint(withdrawTraj, px,py,pz, rx, ry, rz, rw,
                                       1000, 1000, 1000, 1, 1, 1, //torques
                                       false, false, false, false, false,false,//stiff (not torqe)
                                       1.5,fixed_frame_name_);//cumulative time
    arm_->startTrajectory(withdrawTraj);
}


void DoorOpener::grasp_handle(const geometry_msgs::PoseStamped::ConstPtr a_pregrasp,
                              const geometry_msgs::PoseStamped::ConstPtr a_grasp,
                              const geometry_msgs::PoseStamped::ConstPtr a_postgrasp)
{
    if(!started_) return;
    ROS_INFO("Got grasping poses");
    geometry_msgs::Pose postgrasp = (*a_postgrasp).pose;
    geometry_msgs::Pose pregrasp  = (*a_pregrasp).pose;
    geometry_msgs::Pose grasp     = (*a_grasp).pose;

    std::string desired_controller("x_arm_cart_imped_controller");
    desired_controller[0] = side_;
    std::string undesired_controller("x_arm_controller");
    undesired_controller[0] = side_;
    ROS_WARN_COND(!load_controller(desired_controller), "Could not load %s, May already be loaded.", desired_controller.c_str());
    int failure_count = 0;
    while(!switch_controller(desired_controller, undesired_controller))
    {
      failure_count++;
      if(failure_count > 10){
        success_ = false;
        return;
      }

      ROS_ERROR("Could not switch to %s. Waiting 2 sec before retrying. (%d/10 times)", desired_controller.c_str(), failure_count);
      ros::Duration(2).sleep();
      if(!load_controller(desired_controller)){
        ROS_DEBUG("Could not (re-)load %s, May already be loaded.", desired_controller.c_str());
      }

    }

    ee_cart_imped_msgs::EECartImpedGoal toPregrasp, toGrasp, toPostgrasp, backToGrasp;//,traj_l;

    ROS_INFO("opening right gripper");
    use_gripper(0.08, -1.0 );
    // go to pregrasp
    EECartImpedArm::addTrajectoryPoint(toPregrasp, pregrasp.position.x,pregrasp.position.y,pregrasp.position.z,
                                       pregrasp.orientation.x, pregrasp.orientation.y, pregrasp.orientation.z, pregrasp.orientation.w,
                                       1000, 1000, 1000,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       4,fixed_frame_name_); //cumulative time
    {
      EECartImpedClient* traj_client = arm_->startTrajectory(toPregrasp);
      actionlib::SimpleClientGoalState state = traj_client->getState();
      if (state != actionlib::SimpleClientGoalState::SUCCEEDED)
        return;
    }
    if( !wait_for_gripper()) return;
    // go to grasp position
    EECartImpedArm::addTrajectoryPoint(toGrasp, grasp.position.x,grasp.position.y,grasp.position.z,
                                       grasp.orientation.x, grasp.orientation.y, grasp.orientation.z, grasp.orientation.w,
                                       1000, 1000, 1000,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       1,fixed_frame_name_);//cumulative time
    {
      EECartImpedClient* traj_client = arm_->startTrajectory(toGrasp);
      actionlib::SimpleClientGoalState state = traj_client->getState();
      if (state != actionlib::SimpleClientGoalState::SUCCEEDED)
        return;
    }

    ROS_INFO("closing right gripper");
    use_gripper(0.005, 50.0 );
    if(wait_for_gripper()){
      ROS_WARN("Closed completely. Missed handle?");//needs to be closed
    }
    // go to postgrasp position
    EECartImpedArm::addTrajectoryPoint(toPostgrasp, postgrasp.position.x,postgrasp.position.y,postgrasp.position.z,
                                       postgrasp.orientation.x, postgrasp.orientation.y, postgrasp.orientation.z, postgrasp.orientation.w,
                                       500, 100, 100,//force: down and forward
                                       70, 70, 70, //torques: Be wobbly around vertical axis
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       2,fixed_frame_name_);//cumulative time
    {
      EECartImpedClient* traj_client = arm_->startTrajectory(toPostgrasp);
      actionlib::SimpleClientGoalState state = traj_client->getState();
      if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
        success_ = true;
    }
    use_gripper(0.08, -1 );
    // go back to grasp position
    EECartImpedArm::addTrajectoryPoint(backToGrasp, grasp.position.x,grasp.position.y,grasp.position.z,
                                       grasp.orientation.x, grasp.orientation.y, grasp.orientation.z, grasp.orientation.w,
                                       500, 500, 500,//forces
                                       20, 20, 20, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       2,fixed_frame_name_);//cumulative time
/*
    EECartImpedArm::addTrajectoryPoint(backToGrasp, pregrasp.position.x,pregrasp.position.y,pregrasp.position.z,
                                       pregrasp.orientation.x, pregrasp.orientation.y, pregrasp.orientation.z, pregrasp.orientation.w,
                                       1000, 1000, 1000,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       4,fixed_frame_name_); //cumulative time
                                       */
    {
      EECartImpedClient* traj_client = arm_->startTrajectory(backToGrasp);
      actionlib::SimpleClientGoalState state = traj_client->getState();
    }
    wait_for_gripper();//needs to be open to withdraw

    //withdraw(-.04);//go forward
    //push_door();
    //withdraw(.0);//20 cm
    //home_position();
    //go to middle of robot FIXME: Assuming torso frame
    swing_door(*a_grasp);
    started_ = false;
}

tf::StampedTransform DoorOpener::getTransform(const std::string& from, const std::string& to){
    tf::StampedTransform transform;
    ros::Time now = ros::Time::now();
    while(!tflistener_->waitForTransform(from, to,now, ros::Duration(6.0/30.0))){
      ROS_WARN_STREAM("No tranform from "<<from<<" to "<<to<<" available, when getting initial gripper position");
    }
    tflistener_->lookupTransform(from, to, now, transform);
    return transform;
}

void DoorOpener::addTrajectoryPoint(ee_cart_imped_msgs::EECartImpedGoal& msg, tf::Transform pose, float fx, float fy, float fz, float tx, float ty, float tz, float time, std::string frame_id)
{
    EECartImpedArm::addTrajectoryPoint(msg, 
                                       pose.getOrigin().getX(), pose.getOrigin().getY(), pose.getOrigin().getZ(), 
                                       pose.getRotation().getX(), pose.getRotation().getY(), pose.getRotation().getZ(), pose.getRotation().getW(), 
                                       fx, fy, fz,//forces
                                       tx, ty, tz, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       time, frame_id);//cumulative time

}

bool DoorOpener::swing_door(geometry_msgs::PoseStamped start_pose)
{
    tf::Stamped<tf::Transform> tf_start_pose_stamped;
    tf::Transform tf_start_pose;
    tf::poseStampedMsgToTF(start_pose, tf_start_pose_stamped);
    tf::Transform tf_centerwards(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0.20));
    tf_start_pose = tf_start_pose_stamped * tf_centerwards;
    tf::Transform tf_forward(tf::Quaternion(0,0,0,1), tf::Vector3(0.1,0,0));
    tf::Transform tf_touch_pose = tf_start_pose * tf_forward;
    tf::Transform tf_fast_forward(tf::Quaternion(0,0,0,1), tf::Vector3(0.24,0,0));
    tf::Transform tf_swing_pose = tf_start_pose * tf_fast_forward;
    
    ee_cart_imped_msgs::EECartImpedGoal door;

    // go to pretouch
    tf::Quaternion r(tf::Vector3(0,1,0), -M_PI/2.0);//gripper rotation
    addTrajectoryPoint(door, tf_start_pose, 1000, 1000, 1000, 30, 30, 30, 2, start_pose.header.frame_id);
    addTrajectoryPoint(door, tf_touch_pose,  100,  100,  100, 30, 30, 30, 6, start_pose.header.frame_id);
    addTrajectoryPoint(door, tf_swing_pose, 1000, 1000, 1000, 30, 30, 30, 7, start_pose.header.frame_id);
    addTrajectoryPoint(door, tf_start_pose, 1000, 1000, 1000, 30, 30, 30, 10, start_pose.header.frame_id);


    EECartImpedClient* traj_client = arm_->startTrajectory(door);
    actionlib::SimpleClientGoalState state = traj_client->getState();
    return true;
}
void printTransform(const tf::Transform& t, const std::string& name){
    float px = t.getOrigin().getX();
    float py = t.getOrigin().getY();
    float pz = t.getOrigin().getZ();
    float rx = t.getRotation().getX();
    float ry = t.getRotation().getY();
    float rz = t.getRotation().getZ();
    float rw = t.getRotation().getW();
    ROS_INFO_STREAM("Transformation "<<name<<": ("<<px<<", "<<py<<", "<<pz<<") ["<<rx<<", "<<ry<<", "<<rz<<", "<<rw<<"]");
}