void HapticsPSM::get_current_position(tf::Vector3 &v){
v.setValue(group->getCurrentPose().pose.position.x,
group->getCurrentPose().pose.position.y,
group->getCurrentPose().pose.position.z);
}
void HapticsPSM::run_haptic_alg(){
if(check_collison()){
//Step 1:
get_collision_normals_and_points(coll_res.contacts, coll_psm.cur_normal_arr, coll_psm.cur_contact_pnts_arr, coll_psm.insertion_depths);
if (coll_psm.contact_cnts_prev != coll_res.contacts.size()){
ROS_INFO("Number of Contact Points = %i", coll_res.contacts.size());
}
//Step 2:
compute_average_position(coll_psm.cur_contact_pnts_arr, coll_psm.locked_position);
compute_average_normal(coll_psm.cur_normal_arr, coll_psm.cur_normal);
if(coll_psm._first_contact){
ROS_INFO("First contact occured");
tf::Vector3 diff, cur_pos;
get_current_position(cur_pos);
diff = coll_psm.locked_position - cur_pos;
coll_psm.spr_radius = diff.length();
ROS_INFO("SPR Radius Detected to be %f",coll_psm.spr_radius);
coll_psm._first_contact = false;
ROS_INFO("Normal is nx = %f ny = %f nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
}
//Step 3:
if(has_normal_changed(coll_psm.cur_normal, coll_psm.pre_normal)){
// ROS_INFO("Normal has Changed nx = %f ny = %f nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
// ROS_INFO("Normal has Changed pnx = %f pny = %f pnz = %f", coll_psm.pre_normal.getX(),coll_psm.pre_normal.getY(),coll_psm.pre_normal.getZ());
}
//Step 5:
compute_total_deflection(coll_psm.def_total);
//Step 6:
compute_deflection_along_normal(coll_psm.cur_normal, coll_psm.def_total, coll_psm.def_along_n);
//Step 7:
compute_deflection_force(spr_haptic_force.wrench,coll_psm.def_along_n);
//Step 8:
compute_force_in_tip_frame(spr_haptic_force.wrench);
//Step 9:
coll_psm.pre_normal = coll_psm.cur_normal;
coll_psm.contact_cnts_prev = coll_res.contacts.size();
if (coll_psm.cur_normal.angle(coll_psm.def_along_n) > coll_psm.epsilon){
ROS_INFO("Angle between normal and deflection force %f", coll_psm.cur_normal.angle(coll_psm.def_along_n));
}
mesh_normal.wrench.force.x = coll_psm.cur_normal.getX();
mesh_normal.wrench.force.y = coll_psm.cur_normal.getY();
mesh_normal.wrench.force.z = coll_psm.cur_normal.getZ();
normal_pub.publish(mesh_normal);
}
else{
if(!coll_psm._first_contact){
ROS_INFO("First contact Released");
coll_psm._first_contact = true;
}
coll_psm.def_along_n.setValue(0,0,0);
coll_psm.cur_normal.setValue(0,0,0);
spr_haptic_force.wrench.force.x=0;
spr_haptic_force.wrench.force.y=0;
spr_haptic_force.wrench.force.z=0;
}
haptic_deflection.x = coll_psm.def_along_n.getX();
haptic_deflection.y = coll_psm.def_along_n.getY();
haptic_deflection.z = coll_psm.def_along_n.getZ();
haptic_deflection_pub.publish(haptic_deflection);
spr_haptic_pub.publish(spr_haptic_force);
pose_pub.publish(group->getCurrentPose());
}
void Actuator::executeMotion(const amazon::MoveToGoalConstPtr& goal){
if(goal->moveAction == amazon::MoveToGoal::MOVE_TO_POSE){
ROS_INFO("Moving arm to specified location");
if(moveToPose(goal->movePose, goal->arm))
moveToServer.setSucceeded();
else
moveToServer.setAborted();
}
else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_PICK){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else if(!isInsideShelf(goal->movePose.position, goal->shelf.data.at(0))){
ROS_WARN("Point is not inside specified shelf");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
// make orientation horizontal in front of shelf
shelfPose.orientation.w = 0.7071;
shelfPose.orientation.y = 0.7071;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm) && pushGrip(goal->arm, goal->shelf.data.at(0), goal->movePose.position)){// && moveToPose(goal->movePose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_SHELF){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
// make orientation horizontal in front of shelf
shelfPose.orientation.w = 0.7071;
shelfPose.orientation.y = 0.7071;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_DROP){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else if(!isInsideShelf(rightArm.getCurrentPose().pose.position, goal->shelf.data.at(0))){
ROS_WARN("Robot is not inside specified shelf");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s and then drop location", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
// make orientation horizontal in front of shelf
shelfPose.orientation.w = 0.7071;
shelfPose.orientation.y = 0.7071;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm) && moveToPose(goal->movePose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
else if (goal->moveAction == amazon::MoveToGoal::SCOOP_SHELF){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
shelfPose.position.x -= SCOOP_LENGTH;
// make orientation the same as the slope of shelf
shelfPose.orientation.w = 0.4777;
shelfPose.orientation.x = -0.5214;
shelfPose.orientation.y = 0.5214;
shelfPose.orientation.z = -0.4777;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
}
