GraspResult
GraspTesterWithApproach::getInterpolatedIKForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp,
GraspExecutionInfo &execution_info)
{
//get the grasp pose in the right frame
geometry_msgs::PoseStamped grasp_pose_stamped;
grasp_pose_stamped.pose = grasp.grasp_pose;
grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
grasp_pose_stamped.header.stamp = ros::Time(0);
//use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp
geometry_msgs::Vector3Stamped direction;
direction.header.stamp = ros::Time::now();
direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
std::vector<double> empty;
//remember to pass that we want to flip the trajectory
float actual_approach_distance;
int error_code = mechInterface().getInterpolatedIK(pickup_goal.arm_name,
grasp_pose_stamped,
direction, grasp.desired_approach_distance,
empty,
grasp.pre_grasp_posture,
collisionOperationsForGrasp(pickup_goal, grasp),
linkPaddingForGrasp(pickup_goal, grasp),
true, execution_info.approach_trajectory_,
actual_approach_distance);
ROS_DEBUG_NAMED("manipulation"," Grasp executor approach distance: actual (%f), min(%f) and desired (%f)",
actual_approach_distance, grasp.min_approach_distance, grasp.desired_approach_distance);
if ( actual_approach_distance < grasp.min_approach_distance - EPS)
{
ROS_DEBUG_NAMED("manipulation"," Grasp executor: interpolated IK for grasp below min threshold");
if (execution_info.approach_trajectory_.points.empty())
{
ROS_DEBUG_NAMED("manipulation"," Grasp executor: interpolated IK empty, problem is with grasp location");
if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 1.0, 1.0, 0.0); //yellow
if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED)
return Result(GraspResult::GRASP_IN_COLLISION, true);
else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
return Result(GraspResult::GRASP_OUT_OF_REACH, true);
else return Result(GraspResult::GRASP_UNFEASIBLE, true);
}
if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 1.0); //cyan
if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED)
return Result(GraspResult::PREGRASP_IN_COLLISION, true);
else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
return Result(GraspResult::PREGRASP_OUT_OF_REACH, true);
else return Result(GraspResult::PREGRASP_UNFEASIBLE, true);
}
return Result(GraspResult::SUCCESS, true);
}
object_manipulation_msgs::GraspResult
StandardGraspPerformer::retreat(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp,
GraspExecutionInfo &execution_info)
{
arm_navigation_msgs::OrderedCollisionOperations ord;
arm_navigation_msgs::CollisionOperation coll;
//disable collision between gripper and object
coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
coll.object2 = pickup_goal.collision_object_name;
coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
ord.collision_operations.push_back(coll);
//disable collision between gripper and table
coll.object2 = pickup_goal.collision_support_surface_name;
ord.collision_operations.push_back(coll);
ord.collision_operations = concat(ord.collision_operations,
pickup_goal.additional_collision_operations.collision_operations);
geometry_msgs::Vector3Stamped direction;
direction.header.stamp = ros::Time::now();
direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
//even if the complete retreat trajectory is not possible, execute as many
//steps as we can (pass min_distance = 0)
float retreat_distance = grasp.min_approach_distance;
float actual_distance;
if (!mechInterface().translateGripper(pickup_goal.arm_name, direction,
ord, pickup_goal.additional_link_padding,
retreat_distance, 0, actual_distance))
{
ROS_ERROR(" Grasp executor: failed to retreat gripper at all");
return Result(GraspResult::RETREAT_FAILED, false);
}
if (actual_distance < retreat_distance)
{
ROS_WARN(" Grasp executor: only partial retreat (%f) succeeeded", actual_distance);
return Result(GraspResult::RETREAT_FAILED, false);
}
return Result(GraspResult::SUCCESS, true);
}
object_manipulation_msgs::GraspResult
ReactiveGraspPerformer::nonReactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp,
GraspExecutionInfo &execution_info)
{
arm_navigation_msgs::OrderedCollisionOperations ord;
arm_navigation_msgs::CollisionOperation coll;
coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
//disable collisions between end-effector and table
if (!pickup_goal.collision_support_surface_name.empty())
{
coll.object2 = pickup_goal.collision_support_surface_name;
ord.collision_operations.push_back(coll);
}
//disable collisions between attached object and table
if (!pickup_goal.collision_support_surface_name.empty() && !pickup_goal.collision_object_name.empty())
{
coll.object1 = pickup_goal.collision_object_name;
coll.object2 = pickup_goal.collision_support_surface_name;
ord.collision_operations.push_back(coll);
}
ord.collision_operations = concat(ord.collision_operations,
pickup_goal.additional_collision_operations.collision_operations);
float actual_distance;
if (!mechInterface().translateGripper(pickup_goal.arm_name,
pickup_goal.lift.direction,
ord, pickup_goal.additional_link_padding,
pickup_goal.lift.desired_distance, 0.0, actual_distance))
{
ROS_DEBUG_NAMED("manipulation"," Reactive grasp executor: lift performed no steps");
return Result(GraspResult::LIFT_FAILED, false);
}
if (actual_distance < pickup_goal.lift.min_distance)
{
ROS_DEBUG_NAMED("manipulation"," Reactive grasp executor: lift distance below min threshold ");
return Result(GraspResult::LIFT_FAILED, false);
}
if (actual_distance < pickup_goal.lift.desired_distance)
{
ROS_DEBUG_NAMED("manipulation"," Reactive grasp executor: lift distance below desired, but above min threshold");
}
else
{
ROS_DEBUG_NAMED("manipulation"," Reactive grasp executor: desired lift distance executed");
}
return Result(GraspResult::SUCCESS, true);
}
/*! Disable collisions between end-effector and target
Disables ALL collisions on moved obstacles
*/
arm_navigation_msgs::OrderedCollisionOperations
GraspTesterWithApproach::collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp)
{
arm_navigation_msgs::OrderedCollisionOperations ord;
arm_navigation_msgs::CollisionOperation coll;
coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
if (!pickup_goal.collision_object_name.empty())
{
coll.object2 = pickup_goal.collision_object_name;
ord.collision_operations.push_back(coll);
}
if (pickup_goal.allow_gripper_support_collision)
{
coll.object2 = pickup_goal.collision_support_surface_name;
ord.collision_operations.push_back(coll);
}
coll.object1 = pickup_goal.collision_object_name;
//This disables all collisions with the object. Ignore possible collisions of the robot with the object
//during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable
//collisions just with the forearm, which should work almost always but still theoretially is not the right
//thing to do.
coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
ord.collision_operations.push_back(coll);
for (unsigned int i = 0; i < grasp.moved_obstacles.size(); i++)
{
ROS_DEBUG_NAMED("manipulation"," Disabling all collisions for grasp against moved obstacle %s",
grasp.moved_obstacles[i].collision_name.c_str());
coll.object1 = grasp.moved_obstacles[i].collision_name;
//This disables all collisions with the object. Ignore possible collisions of the robot with the object
//during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable
//collisions just with the forearm, which should work almost always but still theoretially is not the right
//thing to do.
coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
ord.collision_operations.push_back(coll);
}
ord.collision_operations = concat(ord.collision_operations,
pickup_goal.additional_collision_operations.collision_operations);
return ord;
}
void MtkRTSPClient::SenderhandResponse1(int resultCode, char* resultString)
{
bool bErr = false;
do
{
if (NULL == pRtspReqSender)
{
LOG_ERR("error!");
bErr = true;
break;
}
bResponseErr = false;
if (0 != resultCode)/*some error occur*/
{
LOG_ERR("response error! \nresultCode = %d", resultCode);
bErr = true;
bResponseErr = true;
break;
}
SenderType type = pRtspReqSender->getSenderType();
LOG_DEBUG("response send type = %d!", type);
/*
*TO DO: deal the result
*/
if (SENDERTYPE_OPTINON == type)
{
}
else if (SENDERTYPE_DESCRIPTION == type)
{
if (false == handDescription(resultString))
{
LOG_ERR("error");
bErr = true;
break;
}
}
else if (SENDERTYPE_PLAY == type)
{
}
else if (SENDERTYPE_PAUSE == type)
{
}
else if (SENDERTYPE_TEARDOWN == type)
{
}
else if (SENDERTYPE_SETUP== type)
{
if (false == handSetup(resultString))
{
LOG_ERR("error");
bErr = true;
break;
}
}
else if (SENDERTYPE_SETPARAM== type)
{
}
else if (SENDERTYPE_GETPARAM== type)
{
}
else/*wrong type*/
{
bErr = true;
break;
}
}while(0);
if (NULL != resultString)
{
LOG_ERR("resultString=%s", resultString);
delete[] resultString;
resultString = NULL;
}
LOG_DEBUG("bErr = %d", bErr);
if (false == bErr)
{
pRtspReqSender->callNext();
}
else
{
LOG_DEBUG("wake up waiters");
/*wakeup all waiters */
wakeupWaiters();
}
}
object_manipulation_msgs::GraspResult
UnsafeGraspPerformer::approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
const object_manipulation_msgs::Grasp &grasp,
GraspExecutionInfo &execution_info)
{
ROS_DEBUG_NAMED("manipulation", "executing unsafe grasp");
//compute the pre-grasp pose
//get the grasp pose in the right frame
geometry_msgs::PoseStamped grasp_pose_stamped;
grasp_pose_stamped.pose = grasp.grasp_pose;
grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
grasp_pose_stamped.header.stamp = ros::Time(0);
//use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp
geometry_msgs::Vector3Stamped direction;
direction.header.stamp = ros::Time::now();
direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
//make sure the input is normalized
geometry_msgs::Vector3Stamped direction_norm = direction;
direction_norm.vector = mechInterface().normalize(direction.vector);
//multiply the approach direction by the desired length and translate the grasp pose back along it
double desired_trajectory_length = fabs(grasp.desired_approach_distance);
direction_norm.vector.x *= desired_trajectory_length;
direction_norm.vector.y *= desired_trajectory_length;
direction_norm.vector.z *= desired_trajectory_length;
geometry_msgs::PoseStamped pregrasp_pose = mechInterface().translateGripperPose(direction_norm, grasp_pose_stamped,
pickup_goal.arm_name);
//move to the pre-grasp using the Cartesian controller
mechInterface().moveArmToPoseCartesian(pickup_goal.arm_name, pregrasp_pose, ros::Duration(15.0),
.0015, .01, 0.02, 0.16, 0.005, 0.087, 0.1,
execution_info.approach_trajectory_.points.front().positions);
//move to the pre-grasp hand posture
mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP, -1);
//if not using reactive grasping, execute the unnormalized interpolated trajectory from pre-grasp to grasp
if(!pickup_goal.use_reactive_execution)
{
mechInterface().attemptTrajectory(pickup_goal.arm_name, execution_info.approach_trajectory_, true);
mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP, pickup_goal.max_contact_force);
}
//otherwise, call reactive grasping
else
{
geometry_msgs::PoseStamped final_grasp_pose_stamped;
final_grasp_pose_stamped.pose = grasp.grasp_pose;
final_grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
final_grasp_pose_stamped.header.stamp = ros::Time(0);
object_manipulation_msgs::ReactiveGraspGoal reactive_grasp_goal;
reactive_grasp_goal.arm_name = pickup_goal.arm_name;
reactive_grasp_goal.target = pickup_goal.target;
reactive_grasp_goal.final_grasp_pose = final_grasp_pose_stamped;
reactive_grasp_goal.trajectory = execution_info.approach_trajectory_;
reactive_grasp_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
reactive_grasp_goal.pre_grasp_posture = grasp.pre_grasp_posture;
reactive_grasp_goal.grasp_posture = grasp.grasp_posture;
//give the reactive grasp 3 minutes to do its thing
ros::Duration timeout = ros::Duration(180.0);
mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_grasp_goal);
if ( !mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
{
ROS_ERROR(" Reactive grasp timed out");
return Result(GraspResult::GRASP_FAILED, false);
}
object_manipulation_msgs::ReactiveGraspResult reactive_grasp_result =
*mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).getResult();
if (reactive_grasp_result.manipulation_result.value != reactive_grasp_result.manipulation_result.SUCCESS)
{
ROS_ERROR("Reactive grasp failed with error code %d", reactive_grasp_result.manipulation_result.value);
return Result(GraspResult::GRASP_FAILED, false);
}
}
if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 0.0); //green
return Result(GraspResult::SUCCESS, true);
}
