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); }