RobotState::RobotState(const RobotState& other)
: state_(3,0)
{
x(other.x());
y(other.y());
theta(other.theta());
}
bool KDLRobotModel::computeFastIK(
const Eigen::Affine3d& pose,
const RobotState& start,
RobotState& solution)
{
// transform into kinematics frame and convert to kdl
auto* T_map_kinematics = GetLinkTransform(&this->robot_state, m_kinematics_link);
KDL::Frame frame_des;
tf::transformEigenToKDL(T_map_kinematics->inverse() * pose, frame_des);
// seed configuration
for (size_t i = 0; i < start.size(); i++) {
m_jnt_pos_in(i) = start[i];
}
// must be normalized for CartToJntSearch
NormalizeAngles(this, &m_jnt_pos_in);
if (m_ik_solver->CartToJnt(m_jnt_pos_in, frame_des, m_jnt_pos_out) < 0) {
return false;
}
NormalizeAngles(this, &m_jnt_pos_out);
solution.resize(start.size());
for (size_t i = 0; i < solution.size(); ++i) {
solution[i] = m_jnt_pos_out(i);
}
return true;
}
void QtQuickSampleApplicationTest::RobotStateHistoryVisualizeTest()
{
// model init
RobotStateHistoryTest model;
RobotState state;
auto it=0;
// array feltöltése
int arr[] = { 0,1 };
QVector<int> v(2);
qCopy(arr, arr+2, v.begin());
// stream init
QByteArray buffer;
QDataStream stream(&buffer, QIODevice::ReadWrite);
// stream mintaadatokkal való feltöltése
stream << (qint32) 3;
stream << Q_INT64_C(123243);
stream << 1.4;
stream << 1.0;
stream << 1.0;
stream << v;
stream << (qint8) 1;
stream << QString::fromUtf8("nincs hiba");
// state feltöltése a mintaadatokkal
state.ReadFrom(stream);
int actual;
actual = model.visualizeTest(state);
// qtest
QVERIFY2(actual==1 , "Nem futott le a historyHasChanged függvény");
}
void RobotProxy::stop()
{
RobotState newState;
newState.setStatus(RobotState::Status::Stopping);
communication.send(newState);
qDebug() << "Stop parancs elküldve.";
}
void RobotProxy::accelerate()
{
RobotState newState;
newState.setStatus(RobotState::Status::Accelerate);
communication.send(newState);
qDebug() << "Gyorsítási parancs elküldve.";
}
bool Eagle::DoesRobotHaveBall(RobotState robotState, Vector2 ballPos)
{
Vector2 robotPos = robotState.Position();
float angleThresh;
float distanceThresh;
if (robotState.HasBall())
{
angleThresh = M_PI_2;
distanceThresh = 70.0f;
}
else
{
angleThresh = M_PI_4;
distanceThresh = 45.0f;
}
float angleToBall = fmod(robotPos.GetAngleTo(&ballPos), (2*M_PI));
float robotOrientation = fmod(robotState.Orientation(), (2*M_PI));
float distanceToBall = robotPos.Distance(&ballPos);
float orientationDiff = fmod(fabs(robotOrientation - angleToBall), (2*M_PI));
bool withinOrientationThresh = (orientationDiff < angleThresh) || (2*M_PI - orientationDiff < angleThresh);
bool withinProximityThresh = distanceToBall < distanceThresh;
if(withinOrientationThresh && withinProximityThresh)
{
return true;
}
return false;
}
void RobotProxy::selftest()
{
RobotState newState;
newState.setStatus(RobotState::Status::SelfTest);
communication.send(newState);
qDebug() << "Öntesztelési parancs elküldve.";
}
//ForwardNode constructor
ForwardNode::ForwardNode(RobotState rs, int p, AbstractNode* parent) : AbstractNode(rs, prevCost, parent) {
//keep direction of previous RobotState
Direction dir = rs.getRobotDirection();
//update position dependent on direction
Position thisPos = rs.getRobotPosition();
switch (dir) {
case NORTH:
thisPos = Position(rs.getRobotPosition().x, rs.getRobotPosition().y + 1);
break;
case EAST:
thisPos = Position(rs.getRobotPosition().x + 1, rs.getRobotPosition().y);
break;
case SOUTH:
thisPos = Position(rs.getRobotPosition().x, rs.getRobotPosition().y - 1);
break;
case WEST:
thisPos = Position(rs.getRobotPosition().x - 1, rs.getRobotPosition().y);
break;
}
//assign thew new RobotState
curState = RobotState(dir, thisPos);
//Calculate the totalCost using prevCost + heuristic + travelCost
prevCost = p;
heuristic = World::getInstance().calculateHeuristic(curState.getRobotPosition());
travelCost = World::getInstance().getTerrain(curState.getRobotPosition());
offGrid = !World::getInstance().isInWorld(curState.getRobotPosition());
m_isGoal = World::getInstance().isGoal(curState.getRobotPosition());
if (m_isGoal)
travelCost -= 100;
totalCost = heuristic + prevCost + travelCost;
}
void RobotProxy::dataReady(QDataStream &stream)
{
// Új robot állapot érkezett, elmentjük a historyba.
// (Onnan vesszük majd azt is, hogy mi az aktuális állapot.)
RobotState state;
state.ReadFrom(stream);
history.Add(state);
}
void RobotProxy::reset()
{
// Reset parancs küldése.
RobotState newState;
newState.setStatus(RobotState::Status::Reset);
communication.send(newState);
qDebug() << "Reset parancs elküldve.";
}
bool KDLRobotModel::computeIKSearch(
const Eigen::Affine3d& pose,
const RobotState& start,
RobotState& solution)
{
// transform into kinematics and convert to kdl
auto* T_map_kinematics = GetLinkTransform(&this->robot_state, m_kinematics_link);
KDL::Frame frame_des;
tf::transformEigenToKDL(T_map_kinematics->inverse() * pose, frame_des);
// seed configuration
for (size_t i = 0; i < start.size(); i++) {
m_jnt_pos_in(i) = start[i];
}
// must be normalized for CartToJntSearch
NormalizeAngles(this, &m_jnt_pos_in);
auto initial_guess = m_jnt_pos_in(m_free_angle);
auto start_time = smpl::clock::now();
auto loop_time = 0.0;
auto count = 0;
auto num_positive_increments =
(int)((GetSolverMinPosition(this, m_free_angle) - initial_guess) /
this->m_search_discretization);
auto num_negative_increments =
(int)((initial_guess - GetSolverMinPosition(this, m_free_angle)) /
this->m_search_discretization);
while (loop_time < this->m_timeout) {
if (m_ik_solver->CartToJnt(m_jnt_pos_in, frame_des, m_jnt_pos_out) >= 0) {
NormalizeAngles(this, &m_jnt_pos_out);
solution.resize(start.size());
for (size_t i = 0; i < solution.size(); ++i) {
solution[i] = m_jnt_pos_out(i);
}
return true;
}
if (!getCount(count, num_positive_increments, -num_negative_increments)) {
return false;
}
m_jnt_pos_in(m_free_angle) = initial_guess + this->m_search_discretization * count;
ROS_DEBUG("%d, %f", count, m_jnt_pos_in(m_free_angle));
loop_time = to_seconds(smpl::clock::now() - start_time);
}
if (loop_time >= this->m_timeout) {
ROS_DEBUG("IK Timed out in %f seconds", this->m_timeout);
return false;
} else {
ROS_DEBUG("No IK solution was found");
return false;
}
return false;
}
void RobotProxy::turn(qint16 fok)
{
RobotState newState;
newState.setStatus(RobotState::Status::Turn);
newState.setTurn(fok);
communication.send(newState);
qDebug() << "Kanyarodás parancs elküldve.";
}
Diffs Node::getDiffs (const RobotState& last, const RobotState& current)
{
Diffs diffs;
const ros::Time now = ros::Time::now();;
// Use the fact that JointState is empty iff this is the first time around
if (last.isEmpty() || now >= last_keyframe_+keyframe_interval_)
{
last_keyframe_ = now;
if (last.isEmpty())
cerr << "Last is empty, so using all diffs" << endl;
else
cerr << "Keyframe interval elapsed, so saving entire joint state" << endl;
diffs.is_keyframe = true;
for (size_t i=0; i<current.joint_state->position.size(); i++)
{
diffs.new_joint_states.push_back(Diff(i, current.joint_state->position[i]));
}
diffs.pose_diff = true;
diffs.new_pose = current.pose;
}
else
{
for (size_t i=0; i<current.joint_state->position.size(); i++)
{
if (fabs(current.joint_state->position[i]-
last.joint_state->position[i])>distance_threshold_ &&
!joint_ignored_[i])
{
cerr << "Joint " << current.joint_state->name[i] << " value " <<
current.joint_state->position[i] << " differs from " <<
last.joint_state->position[i] << endl;
diffs.new_joint_states.push_back(Diff(i, current.joint_state->position[i]));
}
}
if ((current.pose.get() && !last.pose.get()) ||
(!current.pose.get() && last.pose.get()) ||
((current.pose.get() && last.pose.get() &&
poseDistance(*current.pose, *last.pose)>distance_threshold_)))
{
cerr << "Current pose: " << (bool)current.pose.get() << "; last pose: "
<< (bool)last.pose.get() << endl;
diffs.pose_diff = true;
diffs.new_pose = current.pose;
}
}
return diffs;
}
double JointDistHeuristic::computeJointDistance(
const RobotState& s,
const RobotState& t) const
{
double dsum = 0.0;
for (size_t i = 0; i < s.size(); ++i) {
double dj = (s[i] - t[i]);
dsum += dj * dj;
}
return std::sqrt(dsum);
}
FullBodyState PathPostProcessor::createFBState(const RobotState& robot){
vector<double> l_arm, r_arm, base;
vector<double> obj(6,0);
robot.right_arm().getAngles(&r_arm);
robot.left_arm().getAngles(&l_arm);
ContBaseState c_base = robot.base_state();
c_base.getValues(&base);
FullBodyState state;
state.left_arm = l_arm;
state.right_arm = r_arm;
state.base = base;
ContObjectState obj_state = robot.getObjectStateRelMap();
obj[0] = obj_state.x();
obj[1] = obj_state.y();
obj[2] = obj_state.z();
obj[3] = obj_state.roll();
obj[4] = obj_state.pitch();
obj[5] = obj_state.yaw();
state.obj = obj;
return state;
}
/*! \brief Given two robot states, determine how many interpolation steps there
* should be. The delta step size is based on the RPY resolution of the object
* pose and the XYZ spatial resolution.
*/
int RobotState::numInterpSteps(const RobotState& start, const RobotState& end){
ContObjectState start_obj = start.getObjectStateRelBody();
ContObjectState end_obj = end.getObjectStateRelBody();
ContBaseState start_base = start.base_state();
ContBaseState end_base = end.base_state();
double droll = shortest_angular_distance(start_obj.roll(), end_obj.roll());
double dpitch = shortest_angular_distance(start_obj.pitch(), end_obj.pitch());
double dyaw = shortest_angular_distance(start_obj.yaw(), end_obj.yaw());
double d_rot = max(fabs(droll), fabs(dpitch));
double dbase_theta = shortest_angular_distance(start_base.theta(),
end_base.theta());
ROS_DEBUG_NAMED(MPRIM_LOG, "droll %f, dpitch %f, dyaw %f, dbase_theta %f",
droll, dpitch, dyaw, dbase_theta);
d_rot = max(d_rot, fabs(dyaw));
d_rot = max(d_rot, fabs(dbase_theta));
double d_object = ContObjectState::distance(start_obj, end_obj);
double d_base = ContBaseState::distance(ContBaseState(start.base_state()),
ContBaseState(end.base_state()));
ROS_DEBUG_NAMED(MPRIM_LOG, "dobject %f, dbase %f", d_object, d_base);
double d_dist = max(d_object, d_base);
int rot_steps = static_cast<int>(d_rot/ContObjectState::getRPYResolution());
int dist_steps = static_cast<int>(d_dist/ContBaseState::getXYZResolution());
ROS_DEBUG_NAMED(MPRIM_LOG, "rot steps %d, dist_steps %d", rot_steps, dist_steps);
int num_interp_steps = max(rot_steps, dist_steps);
return num_interp_steps;
}
bool ActionSet::applyMotionPrimitive(const RobotState &state, MotionPrimitive &mp, Action &action)
{
action = mp.action;
for(size_t i = 0; i < action.size(); ++i)
{
if(action[i].size() != state.size())
return false;
/*
ROS_INFO("[action_set] state: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", state[0], state[1], state[2], state[3], state[4], state[5], state[6]);
mp.print();
*/
for(size_t j = 0; j < action[i].size(); ++j)
action[i][j] = angles::normalize_angle(action[i][j] + state[j]);
}
return true;
}
RobotState MotionModel::sampleNextState(const RobotState& state_1, const OdometryReading& odom_1, const OdometryReading& odom_2)
{
// differentials in odometry frame
double d_th_enc_1 = shortest_angular_distance(odom_1.theta, atan2(odom_2.y-odom_1.y, odom_2.x-odom_1.x));
double d_s_enc = sqrt(pow((odom_2.y-odom_1.y), 2) + pow((odom_2.x-odom_1.x), 2));
double d_th_enc_2 = shortest_angular_distance(d_th_enc_1 + odom_1.theta, odom_2.theta);
// standard deviations for differentials' noise
double std_1 = MotionModelParams::alpha_1*d_th_enc_1 + MotionModelParams::alpha_2*d_s_enc;
double std_2 = MotionModelParams::alpha_3*d_s_enc + MotionModelParams::alpha_4*(d_th_enc_1 + d_th_enc_2);
double std_3 = MotionModelParams::alpha_1*d_th_enc_2 + MotionModelParams::alpha_2*d_s_enc;
double d_th_1 = 0.0;
double d_s = 0.0;
double d_th_2 = 0.0;
// differentials in world frame
if (std::fabs(std_1) > 0)
d_th_1 = d_th_enc_1 + distribution_(generator_)*std_1;
if (std::fabs(std_2) > 0)
d_s = d_s_enc + distribution_(generator_)*std_2;
if (std::fabs(std_3) > 0)
d_th_2 = d_th_enc_2 + distribution_(generator_)*std_3;
// sample next state
RobotState state_2;
state_2.x(state_1.x() + d_s*cos(state_1.theta() + d_th_1));
state_2.y(state_1.y() + d_s*sin(state_1.theta() + d_th_1));
state_2.theta(normalize_angle(state_1.theta() + d_th_1 + d_th_2));
if (!map_->isFree(state_2.x(), state_2.y())) {
state_2.x(state_1.x());
state_2.y(state_1.y());
state_2.theta(normalize_angle(state_1.theta() + distribution_(generator_)*
DEG2RAD(10)));
}
return state_2;
}
/*! \brief Given two robot states, we interpolate all steps in between them. The
* delta step size is based on the RPY resolution of the object pose and the XYZ
* resolution of the base. This returns a vector of RobotStates, which are
* discretized to the grid (meaning if the arms move a lot, but the base barely
* moves, the base discrete values will likely stay the same). This determines
* the number of interpolation steps based on which part of the robot moves more
* (arms or base).
* TODO need to test this a lot more
*/
bool RobotState::workspaceInterpolate(const RobotState& start, const RobotState& end,
vector<RobotState>* interp_steps){
ContObjectState start_obj = start.getObjectStateRelBody();
ContObjectState end_obj = end.getObjectStateRelBody();
ContBaseState start_base = start.base_state();
ContBaseState end_base = end.base_state();
int num_interp_steps = numInterpSteps(start, end);
vector<ContObjectState> interp_obj_steps;
vector<ContBaseState> interp_base_steps;
ROS_DEBUG_NAMED(MPRIM_LOG, "start obj");
start_obj.printToDebug(MPRIM_LOG);
ROS_DEBUG_NAMED(MPRIM_LOG, "end obj");
end_obj.printToDebug(MPRIM_LOG);
interp_obj_steps = ContObjectState::interpolate(start_obj, end_obj,
num_interp_steps);
interp_base_steps = ContBaseState::interpolate(start_base, end_base,
num_interp_steps);
assert(interp_obj_steps.size() == interp_base_steps.size());
ROS_DEBUG_NAMED(MPRIM_LOG, "size of returned interp %lu", interp_obj_steps.size());
// should at least return the same start and end poses
if (num_interp_steps < 2){
assert(interp_obj_steps.size() == 2);
} else {
assert(interp_obj_steps.size() == static_cast<size_t>(num_interp_steps));
}
for (size_t i=0; i < interp_obj_steps.size(); i++){
interp_obj_steps[i].printToDebug(MPRIM_LOG);
RobotState seed(interp_base_steps[i], start.right_arm(), start.left_arm());
RobotPosePtr new_robot_state;
if (!computeRobotPose(interp_obj_steps[i], seed, new_robot_state)){
return false;
}
interp_steps->push_back(*new_robot_state);
}
return true;
}
/*!
* Identify the target state that we wish the robot to be in. This will be the target which the A* algorithm plots towards.
*/
RobotState Eagle::IdentifyTarget(RobotState &ourRobotState, RobotState &enemyRobotState, Vector2 ballPos, bool doWeHaveBall, bool &isMovingToBall)
{
RobotState targetState;
Vector2 ourRobotPos = ourRobotState.Position();
Vector2 enemyRobotPos = enemyRobotState.Position();
Vector2 ourGoalCentre = GoalCentrePosition(m_pitchSide);
Vector2 enemyGoalCentre;
if (m_pitchSide == eLeftSide)
{
enemyGoalCentre = GoalCentrePosition(eRightSide);
}
else
{
enemyGoalCentre = GoalCentrePosition(eLeftSide);
}
ballPos.Clamp(Vector2(0,0), Vector2(m_pitchSizeX-1, m_pitchSizeY-1));
isMovingToBall = false;
// doWeHaveBall is a value which comes from the robot's rotational sensors.
//ourRobotState.SetHasBall(doWeHaveBall);
ourRobotState.SetHasBall(DoesRobotHaveBall(ourRobotState, ballPos));
enemyRobotState.SetHasBall(DoesRobotHaveBall(enemyRobotState, ballPos));
if (m_state == ePenaltyAttack)
{
m_isKickingPosSet = false;
// When taking a penalty, we want to find a free position to kick to.
// Position should stay the same - we only want to re-orientate.
targetState.SetPosition(ourRobotPos);
// We'll do this by checking for intersections between us and three positions on the goal line.
Vector2 targetPositions[3];
targetPositions[0] = enemyGoalCentre;
targetPositions[1] = enemyGoalCentre - Vector2(0,50);
targetPositions[2] = enemyGoalCentre + Vector2(0,50);
int arrayLength = sizeof(targetPositions)/sizeof(Vector2);
Vector2 optimalShootingTarget;
float bestDistanceFromEnemy = 0;
// Iterate through the positions, finding the best one, based on if it's unblocked and how far it is from the enemy robot.
for (int i=0; i < arrayLength; i++)
{
// Check if the target is unblocked.
bool isBlocked = m_intersection.LineCircleIntersection(ourRobotPos, targetPositions[i], enemyRobotPos, ROBOT_RADIUS);
if (isBlocked)
{
continue;
}
float distanceSqdToEnemy = enemyRobotPos.DistanceSquared(&targetPositions[i]);
// Check if this beats our previous best distance.
if (distanceSqdToEnemy > bestDistanceFromEnemy)
{
bestDistanceFromEnemy = distanceSqdToEnemy;
optimalShootingTarget = targetPositions[i];
}
// Check that we do actually have a target set.
if (optimalShootingTarget.IsSet())
{
float angleToTarget = ourRobotPos.GetAngleTo(&optimalShootingTarget);
targetState.SetOrientation(angleToTarget);
}
else
{
targetState.SetOrientation(ourRobotState.Orientation());
}
}
}
else if (m_state == ePenaltyDefend)
{
m_isKickingPosSet = false;
// When defending, we're permitted to move up and down the goalline.
// Orientation should stay the same.
targetState.SetOrientation(ourRobotState.Orientation());
// X-axis position should be the same, y-axis should be a position extrapolated in the direction of the enemy robot.
//float extrapolationGradient = tan(enemyRobotState.Orientation());
// I'm experimenting with extrapolating on a line between the robot and ball instead.
float extrapolationGradient = enemyRobotPos.Gradient(&ballPos);
int extrapolatedY = enemyRobotPos.Y() + ((ourRobotPos.X() - enemyRobotPos.X()) * extrapolationGradient);
Vector2 proposedPosition(ourRobotPos.X(), extrapolatedY);
proposedPosition.Clamp(Vector2(0,0), Vector2(m_pitchSizeX-1, m_pitchSizeY-1));
targetState.SetPosition(proposedPosition);
}
else
{
// If we're here, assume we're in open play.
if (!ourRobotState.HasBall())
{
m_isKickingPosSet = false;
// Check if the enemy robot has the ball.
if (enemyRobotState.HasBall())
{
if (m_pitchSide == eLeftSide)
{
targetState.SetPosition(ourGoalCentre.X() + 50, ourGoalCentre.Y());
targetState.SetOrientation(0);
}
else
{
targetState.SetPosition(ourGoalCentre.X() - 50, ourGoalCentre.Y());
targetState.SetOrientation(M_PI);
}
}
else
{
// If we don't have the ball, the aim should be to move to the ball.
isMovingToBall = true;
targetState.SetPosition(ballPos);
if (m_pitchSide == eLeftSide)
{
targetState.SetOrientation(0);
}
else
{
targetState.SetOrientation(M_PI);
}
}
}
else
{
// Check if the previously calculated target pos is appropriate or if we need to recalc.
if ((m_isKickingPosSet) && ((m_kickingPos.Distance(&ourRobotPos) < 30) || (m_kickingPos.Distance(&enemyRobotPos) < 40)))
{
m_isKickingPosSet = false;
}
if (!m_isKickingPosSet)
{
// If we have the ball, let's move to a more appropriate place.
// This is done regardless of whether we're shooting or not.
/*
This should depend on:
1. The opposite half of the pitch to the enemy robot.
2. Within the kicking threshold.
3. Fairly central, but outside of the enemy robot's radius
4. Orientated towards the goal
*/
bool isEnemyOnBottomSide;
// Determine which half of the pitch the enemy robot is on.
if (enemyRobotState.Position().Y() < m_pitchSizeY/2)
{
isEnemyOnBottomSide = true;
}
else
{
isEnemyOnBottomSide = false;
}
// Determine where the kicking threshold is for this side of the pitch.
float kickingPositionX;
float kickingPositionY;
if (m_pitchSide == eLeftSide)
{
kickingPositionX = m_pitchSizeX - ((KICKING_THRESHOLD/2)*m_pitchSizeX);
}
else
{
kickingPositionX = (KICKING_THRESHOLD/2)*m_pitchSizeX;
}
if (isEnemyOnBottomSide)
{
kickingPositionY = m_pitchSizeY/2 + 80;
}
else
{
kickingPositionY = m_pitchSizeY/2 - 80;
}
// Check if the position is within two robot radii of the enemy.
Vector2 proposedPosition = Vector2(kickingPositionX,kickingPositionY);
// Check if the proposed position is too close to the enemy robot to be used.
if (proposedPosition.Distance(&enemyRobotPos) < 2*ROBOT_RADIUS)
{
float adjustedYPosition;
if (isEnemyOnBottomSide)
{
adjustedYPosition = proposedPosition.Y() + 2*ROBOT_RADIUS;
}
else
{
adjustedYPosition = proposedPosition.Y() - 2*ROBOT_RADIUS;
}
proposedPosition = Vector2( proposedPosition.X(), adjustedYPosition);
}
proposedPosition.Clamp(Vector2(0,0), Vector2(m_pitchSizeX-1, m_pitchSizeY-1));
m_kickingPos = proposedPosition;
m_isKickingPosSet = true;
}
targetState.SetPosition(m_kickingPos);
// We'll do this by checking for intersections between us and three positions on the goal line.
Vector2 targetPositions[3];
targetPositions[0] = enemyGoalCentre;
targetPositions[1] = enemyGoalCentre - Vector2(0,50);
targetPositions[2] = enemyGoalCentre + Vector2(0,50);
int arrayLength = sizeof(targetPositions)/sizeof(Vector2);
Vector2 optimalShootingTarget;
float bestDistanceFromEnemy = 0;
// Iterate through the positions, finding the best one, based on if it's unblocked and how far it is from the enemy robot.
for (int i=0; i < arrayLength; i++)
{
// Check if the target is unblocked.
bool isBlocked = m_intersection.LineCircleIntersection(m_kickingPos, targetPositions[i], enemyRobotPos, ROBOT_RADIUS);
if (isBlocked)
{
continue;
}
float distanceSqdToEnemy = enemyRobotPos.DistanceSquared(&targetPositions[i]);
// Check if this beats our previous best distance.
if (distanceSqdToEnemy > bestDistanceFromEnemy)
{
bestDistanceFromEnemy = distanceSqdToEnemy;
optimalShootingTarget = targetPositions[i];
}
// Check that we do actually have a target set.
if (optimalShootingTarget.IsSet())
{
float angleToTarget = m_kickingPos.GetAngleTo(&optimalShootingTarget);
targetState.SetOrientation(angleToTarget);
}
else
{
targetState.SetOrientation(ourRobotState.Orientation());
}
}
}
targetState.SetPosition((int)targetState.Position().X(), (int)targetState.Position().Y());
}
return targetState;
}
bool Eagle::ShouldWeShoot(RobotState ourRobotState, RobotState enemyRobotState, Vector2 ballPos)
{
// In simplistic terms, we should shoot if we're close enough to the goal,
// we've got the ball, and there is clear line-of-sight to goal
Vector2 goalPosition;
if (m_pitchSide == eLeftSide)
{
goalPosition = GoalCentrePosition(eRightSide);
}
else
{
goalPosition = GoalCentrePosition(eLeftSide);
}
Vector2 ourRobotPos = ourRobotState.Position();
const float angleThresh = M_PI_4;
float distToGoal = ourRobotState.Position().Distance(&goalPosition);
// Ian reckons this should be 1/4 of the pitch, but I'm living dangerously and have gone for 1/3.
float distanceThreshold = KICKING_THRESHOLD * m_pitchSizeX;
// Commenting this out now - we need to account for the ball being invisible.
//bool weHaveBall = DoWeHaveBall(ourRobotState, ballPos);
bool closeToGoal = distToGoal < distanceThreshold;
// Check that the enemy robot isn't obstructing the ball.
bool isGoalClear = false;
float angleToGoal = fmod(ourRobotPos.GetAngleTo(&goalPosition), 2*M_PI);
float ourOrientation = fmod(ourRobotState.Orientation(), 2*M_PI);
float angleDifference = fmod(fabs(angleToGoal - ourOrientation), 2*M_PI);
//bool isFacingGoal = (angleDifference < angleThresh) || (angleDifference > (2*M_PI) - angleThresh);
// Check if we're facing somewhere into the goal.
// Extrapolate to the goal-line position we're facing.
float extrapolatedYCoord = ourRobotPos.Y() + (ourRobotPos.Gradient(&ballPos) * (goalPosition.X() - ourRobotPos.X()));
// The goal is in the middle of the pitch, constituting half its length.
float goalTop = 0.75 * m_pitchSizeY;
float goalBottom = 0.25 * m_pitchSizeY;
bool isFacingGoal = (extrapolatedYCoord >= goalBottom) && (extrapolatedYCoord <= goalTop);
// The position we're aiming for.
Vector2 goalTarget(m_pitchSizeX, extrapolatedYCoord);
if (!m_intersection.LineCircleIntersection(ourRobotPos, goalTarget, enemyRobotState.Position(), ROBOT_RADIUS))
{
isGoalClear = true;
}
if (closeToGoal && isGoalClear && isFacingGoal)
{
return true;
}
return false;
}
// this is a bit weird at the moment, but we use the arm angles as seed angles
// disc_obj_state is in body frame : Torso_lift_link?
bool RobotState::computeRobotPose(const DiscObjectState& disc_obj_state,
const RobotState& seed_robot_pose,
RobotPosePtr& new_robot_pose,
bool free_angle_search){
ContObjectState obj_state = disc_obj_state.getContObjectState();
RightContArmState seed_r_arm = seed_robot_pose.right_arm();
LeftContArmState seed_l_arm = seed_robot_pose.left_arm();
KDL::Frame obj_frame;
obj_frame.p.x(obj_state.x());
obj_frame.p.y(obj_state.y());
obj_frame.p.z(obj_state.z());
obj_frame.M = KDL::Rotation::RPY(obj_state.roll(),
obj_state.pitch(),
obj_state.yaw());
KDL::Frame r_obj_to_wrist_offset = seed_robot_pose.right_arm().getObjectOffset();
KDL::Frame l_obj_to_wrist_offset = seed_robot_pose.left_arm().getObjectOffset();
KDL::Frame r_wrist_frame = obj_frame * r_obj_to_wrist_offset;
KDL::Frame l_wrist_frame = obj_frame * l_obj_to_wrist_offset;
// store the seed angles as the actual angles in case we don't compute for
// the other arm. otherwise, computing a new robot pose would reset the
// unused arm to angles of 0
vector<double> r_seed(7,0), r_angles(7,0), l_seed(7,0), l_angles(7,0);
seed_r_arm.getAngles(&r_seed);
r_angles = r_seed;
seed_l_arm.getAngles(&l_seed);
l_angles = l_seed;
ik_calls++;
struct timeval tv_b;
struct timeval tv_a;
gettimeofday(&tv_b, NULL);
double before = tv_b.tv_usec + (tv_b.tv_sec * 1000000);
gettimeofday(&tv_a, NULL);
// decide which arms we need to run IK for
bool use_right_arm = (m_planning_mode == PlanningModes::RIGHT_ARM ||
m_planning_mode == PlanningModes::DUAL_ARM ||
m_planning_mode == PlanningModes::RIGHT_ARM_MOBILE ||
m_planning_mode == PlanningModes::DUAL_ARM_MOBILE);
bool use_left_arm = (m_planning_mode == PlanningModes::LEFT_ARM ||
m_planning_mode == PlanningModes::DUAL_ARM ||
m_planning_mode == PlanningModes::LEFT_ARM_MOBILE ||
m_planning_mode == PlanningModes::DUAL_ARM_MOBILE);
if (!(use_right_arm || use_left_arm)){
ROS_ERROR("what! not using any arm for IK??");
}
#ifdef USE_KDL_SOLVER
if (use_right_arm) {
SBPLArmModelPtr arm_model = seed_r_arm.getArmModel();
bool ik_success = arm_model->computeFastIK(r_wrist_frame, r_seed, r_angles);
if (!ik_success) {
ROS_DEBUG_NAMED(MPRIM_LOG, "IK failed for right arm");
return false;
}
}
if (use_left_arm) {
SBPLArmModelPtr arm_model = seed_l_arm.getArmModel();
bool ik_success = arm_model->computeFastIK(l_wrist_frame, l_seed, l_angles);
if (!ik_success){
ROS_DEBUG_NAMED(MPRIM_LOG, "IK failed for left arm");
return false;
}
}
#endif
#ifdef USE_IKFAST_SOLVER
if (use_right_arm){
double r_free_angle = r_seed[Joints::UPPER_ARM_ROLL];
if (!m_ikfast_solver.ikRightArm(r_wrist_frame, r_free_angle, &r_angles)){
return false;
}
}
if (use_left_arm){
double l_free_angle = l_seed[Joints::UPPER_ARM_ROLL];
if (!m_ikfast_solver.ikLeftArm(l_wrist_frame, l_free_angle, &l_angles)){
return false;
}
}
#endif
double after = tv_a.tv_usec + (tv_a.tv_sec * 1000000);
ik_time += after - before;
new_robot_pose = make_shared<RobotState>(seed_robot_pose.base_state(),
RightContArmState(r_angles),
LeftContArmState(l_angles));
return true;
}
void Connector::readSocketWorker()
{
ros::Rate rate = ros::Rate(readFrequency);
while(runReadSocketThread && socket.is_open())
{
try
{
boost::system::error_code error;
//===========================
// 1. read package size
//===========================
char dataPackageSize[4];
size_t length = socket.read_some(boost::asio::buffer(dataPackageSize), error); //reads only 4 bytes of overall comming package size
//connection closed cleanly by peer.
if (error == boost::asio::error::eof)
{
socket.close();
break;
}
//some other error
else if (error)
{
throw boost::system::system_error(error);
}
//verify read byte length
if (length != 4)
{
throw length_error("socket read: data package size: read byte length (" + boost::lexical_cast<string>(length) + ") differs from expected length (4)");
}
// Calculate packagesize
uint32_t packageSize = (dataPackageSize[3] << 24) | (dataPackageSize[2] << 16) | (dataPackageSize[1] << 8) | dataPackageSize[0];
packageSize = be32toh(packageSize) - 4;
//print dataPackageContent stream in hex format
ROS_DEBUG_NAMED("connector", "socket read: data package size (%i): %s", (int)length, hexString(dataPackageSize, length).c_str());
//===========================
// 2. read package content
//===========================
char dataPackageContent[1024];
length = socket.read_some(boost::asio::buffer(dataPackageContent), error); // reads the rest of the package
//connection closed cleanly by peer.
if (error == boost::asio::error::eof)
{
break;
}
//some other error
else if (error)
{
throw boost::system::system_error(error);
}
//verify read byte length
if (length != packageSize) //throws if read package size does not match with received package size
{
throw length_error("socket read: data package content: read byte length (" + boost::lexical_cast<string>(length) + ") differs from expected length (" + boost::lexical_cast<string>(packageSize) + ")");
}
//print dataPackageContent stream in hex format, and only the content part (without the first 4byte package size info!)
ROS_DEBUG_NAMED("connector", "socket read: data package content (%i): %s", (int)length, hexString(dataPackageContent, length).c_str());
if (port == PRIMARY)
{
//TODO support port
ROS_WARN("port %i not supported", port);
}
else if (port == SECONDARY)
{
ROS_WARN_COND(dataPackageContent[0] != 16, "Something went wrong: First packageType must always be 16 but was %i", dataPackageContent[0]);
//process data package
RobotState robotState;
JointPosition jointPosition(6);
JointVelocity jointVelocity(6);
CartesianPosition cartesianPosition;
int bytepointer = 1; // first byte of message was consumed as RobotMessageType
while (bytepointer < packageSize)
{
Packet_port30002::PacketHeader *packageHeader = (Packet_port30002::PacketHeader*)&dataPackageContent[bytepointer];
packageHeader->fixByteOrder();
//ROS_WARN("Type: %i, Length: %i", packageHeader->packageType, packageHeader->packageLength);
switch (packageHeader->packageType)
{
case 0:
{
Packet_port30002::RobotMode *robotmode = (Packet_port30002::RobotMode*)&dataPackageContent[bytepointer+sizeof(Packet_port30002::PacketHeader)];
ROS_DEBUG("RobotMode Package received");
robotmode->fixByteOrder();
robotState.isUrProgramRunning = robotmode->isProgramRunning;
robotState.isUrProgramPaused = robotmode->isProgramPaused;
break;
}
case 1:
{
for (int jointCnt=0; jointCnt<6; jointCnt++)
{
Packet_port30002::Joint *jointData =
(Packet_port30002::Joint*)&dataPackageContent[bytepointer+sizeof(Packet_port30002::PacketHeader)+jointCnt*sizeof(Packet_port30002::Joint)];
jointData->fixByteOrder();
jointPosition[jointCnt] = jointData->q_act;
jointVelocity[jointCnt] = jointData->qd_act;
}
ROS_DEBUG("JointData Package received");
break;
}
case 2:
{
Packet_port30002::ToolData *toolData = (Packet_port30002::ToolData*)&dataPackageContent[bytepointer+sizeof(Packet_port30002::PacketHeader)];
ROS_DEBUG("ToolData Package received");
// Do the following before using it!
//toolData->fixByteOrder();
break;
}
case 3:
{
Packet_port30002::MasterboardData *masterBoardData = (Packet_port30002::MasterboardData*)&dataPackageContent[bytepointer+sizeof(Packet_port30002::PacketHeader)];
masterBoardData->fixByteOrder();
ROS_DEBUG("MasterboardData Package received");
// Read Input State 0 to 7 -> robotState.getIO(0..7)
for (int i=0; i<8; i++)
{
robotState.set_IO(i, masterBoardData->bit_to_bool(masterBoardData->DigitalnputBits, i));
ROS_INFO("Input State %d: %d", i, robotState.get_IO(i));
}
// Read Output State 0 to 7 -> robotState.getIO(8..15)
for (int i=0; i<8; i++)
{
robotState.set_IO(18+i, masterBoardData->bit_to_bool(masterBoardData->DigitaOutputBits, i));
ROS_INFO("Output State %d: %d", 18 + i, robotState.get_IO(18+i));
}
// TODO: Read Tool inputs and outputs??? The following worked in the past but not with CB3.2
// IOS
// 0-7 digital input, 8-15 configurable input, 16-17 tool input, 18-25 digital output, 26-33 configurable output, 34-35 tool output
/* for (int i= 0; i< 8; i++) robotState.set_IO(i, packet->bit_to_bool(dataPackageContent, 452, i%8));
for (int i= 8; i<16; i++) robotState.set_IO(i, packet->bit_to_bool(dataPackageContent, 451, i%8));
for (int i=16; i<18; i++) robotState.set_IO(i, packet->bit_to_bool(dataPackageContent, 450, i%2));
for (int i=18; i<26; i++) robotState.set_IO(i, packet->bit_to_bool(dataPackageContent, 456, i%8));
for (int i=26; i<34; i++) robotState.set_IO(i, packet->bit_to_bool(dataPackageContent, 455, i%8));
for (int i=34; i<36; i++) robotState.set_IO(i, packet->bit_to_bool(dataPackageContent, 454, i%2)); */
break;
}
case 4:
{
Packet_port30002::CartesianInfo *cartesianInfo = (Packet_port30002::CartesianInfo*)&dataPackageContent[bytepointer+sizeof(Packet_port30002::PacketHeader)];
ROS_DEBUG("CartesianInfo Package received");
cartesianInfo->fixByteOrder();
cartesianPosition.setValues(
cartesianInfo->X_Tool,
cartesianInfo->Y_Tool,
cartesianInfo->Z_Tool,
cartesianInfo->Rx,
cartesianInfo->Ry,
cartesianInfo->Rz);
break;
}
}
bytepointer+=packageHeader->packageLength;
}
robotState.setJointPosition(jointPosition);
robotState.setJointVelocity(jointVelocity);
robotState.setCartesianPosition(cartesianPosition);
notifyListeners(robotState);
}
else if (port == REALTIME)
{
//TODO support port
ROS_WARN("port %i not supported", port);
}
if (readFrequency > 0)
{
rate.sleep();
}
}
catch (std::exception& e)
{
ROS_WARN_NAMED("connector", "error in read socket thread: %s", e.what());
}
}
ROS_DEBUG_NAMED("connector", "exit readSocketWorker thread");
}
