door_swinger.cpp

Go to the documentation of this file.

#include "door_swinger.h"
// arm movement
#include <ee_cart_imped_action/ee_cart_imped_arm.hh>
#include <ee_cart_imped_msgs/EECartImpedGoal.h>

#include <cmath>  //for fabs
// arm movement

DoorSwinger::DoorSwinger(char left_right): 
    serviceServer(n.advertiseService("swing_door", &DoorSwinger::swing_door, this)),
    controller_name_("/l_arm_cart_imped_controller")
{
  std::string client_name = "/l_gripper_controller/gripper_action";
  if(left_right == 'r') client_name[1] = left_right;
  if(left_right == 'r') controller_name_[1] = left_right;

  gripperClient = new gripActClient(client_name, true); 
  //wait for the gripper action server to come up
  ROS_INFO("door_swinger/swing_door service ready");
}

DoorSwinger::~DoorSwinger(){
  delete gripperClient;
}

void DoorSwinger::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 DoorSwinger::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_ERROR("Failed to use gripper"); 
        return false;
    }
}

void DoorSwinger::home_position()
{
    EECartImpedArm arm_r(controller_name_);
    ee_cart_imped_msgs::EECartImpedGoal homePos;//,traj_l;

    tf::Quaternion r(tf::Vector3(1,0,0), -M_PI/2.0);//gripper rotation
    EECartImpedArm::addTrajectoryPoint(homePos, 0.65, -0.00,0.20, r.getX(), r.getY(), r.getZ(), r.getW(),
                                       500, 500, 500, 30, 30, 30, //torques
                                       false, false, false, false, false,false,//stiff (not torqe)
                                       2, "/torso_lift_link");//cumulative time
    arm_r.startTrajectory(homePos);
}

void DoorSwinger::withdraw(float meters)
{
    // tf stuff
    tf::StampedTransform gripper_transform;
    //wait for transformation of gripper
    ros::Time now = ros::Time::now();
    while(!tflistener.waitForTransform("/torso_lift_link", "/r_gripper_tool_frame",now, ros::Duration(6.0/30.0))){
      ROS_WARN("No tranform from /torso_lift_link to /r_gripper_tool_frame available");
      now = ros::Time::now();
    }
    tflistener.lookupTransform("/torso_lift_link", "/r_gripper_tool_frame", now, gripper_transform);
    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 arm_r(controller_name_);
    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, "/torso_lift_link");//cumulative time
    arm_r.startTrajectory(withdrawTraj);
}

bool DoorSwinger::swing_door(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
    ee_cart_imped_msgs::EECartImpedGoal door;

    //ROS_INFO("closing gripper");
    //use_gripper(0.00, -1.0 );
    //home_position();
    // go to pregrasp
    tf::Quaternion r(tf::Vector3(0,1,0), -M_PI/2.0);//gripper rotation
    /*
    EECartImpedArm::addTrajectoryPoint(door, 0.15, +0.2, -0.1,r.getX(), r.getY(), r.getZ(), r.getW(),
                                       200, 200, 200,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       3, "/checkerboard_0"); //cumulative time
                                       */
    // go to middle of door
    EECartImpedArm::addTrajectoryPoint(door, 0.00, 0.0, 0.15, 0,0,0,1,
                                       1000, 1000, 300,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       3, "/torso_lift_link");//cumulative time

    // touch
    EECartImpedArm::addTrajectoryPoint(door, 0.10, 0.0, 0.0, 0,0,0,1,
                                       100, 100, 30,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       8, "/l_gripper_l_finger_tip_link");//cumulative time

    // push
    EECartImpedArm::addTrajectoryPoint(door, 0.20, 0.0, 0.0, 0,0,0,1,
                                       1000, 1000, 1000,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       9, "/l_gripper_l_finger_tip_link");//cumulative time

    /*
    EECartImpedArm::addTrajectoryPoint(door, 0.15, +0.2, 0.28, r.getX(), r.getY(), r.getZ(), r.getW(),
                                       1000, 1000, 1000,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       6, "/checkerboard_0");//cumulative time
   */
    /* go back to pregrasp
    EECartImpedArm::addTrajectoryPoint(door, 0.05, -0.3, -0.1,r.getX(), r.getY(), r.getZ(), r.getW(),
                                       100, 100, 100,//forces
                                       30, 30, 30, //torques
                                       false, false, false,//stiff (not force)
                                       false, false,false,//stiff (not torqe)
                                       9, "/checkerboard_0"); //cumulative time
    */
    //home

    EECartImpedArm arm_r(controller_name_);
    EECartImpedClient* traj_client = arm_r.startTrajectory(door);
    actionlib::SimpleClientGoalState state = traj_client->getState();
    // The door might still be open, so don't signal failure
    //if (state != actionlib::SimpleClientGoalState::SUCCEEDED)
      //return false;
    wait_for_gripper();

    //withdraw(.2);//10 cm
    //home_position();
    return true;
}