void VO2Controller::render(HGE * hge) { Mover::Driver::render(hge); float size = 5 * mover->getMaster()->getSphereSize(); Pose pose = mover->getGlobalPose(); for(auto it=obstacles.begin();it!=obstacles.end();++it) { VelocityObstacle vo(pose.getPosition(),size,it->first.getPosition(0),it->second,it->first.velocity); drawVO(hge,vo, size, 10); } drawRays(hge, pose.getPosition(), rays); // // //for(auto it=segments.begin();it!=segments.end();++it) // // drawArc(object->position,object->maxVelocity*1.1,*it); }
bool EvaluationModule::checkAngleToPass(Vector2D targetPosition, Pose currRobotPosition, double & angleToTarget) const{ angleToTarget = calculateAngleToTarget( currRobotPosition, Pose(targetPosition.x,targetPosition.y,0) ); double threshold = 0.017 * currRobotPosition.getPosition().distance(targetPosition); //LOG_INFO(this->log,"set threshold to "<<threshold); if( fabs( angleToTarget ) < threshold ){//1stopien return true; } return false; }
Task::status Rotate::run(void * arg, int steps){ GameStatePtr currGameState( new GameState ); double lastSimTime=0; double currSimTime=0; currSimTime=video.updateGameState(currGameState); //biezaca pozycja robota Pose currRobotPose = (*currGameState).getRobotPos( robot->getRobotID() ); if(this->targetRobotId != Robot::unknown) targetPosition = (*currGameState).getRobotPos(this->targetRobotId).getPosition(); while( !this->stopTask && (steps--)!=0 ){ if( lastSimTime < ( currSimTime=video.updateGameState(currGameState) ) ){ lastSimTime = currSimTime; currRobotPose=currGameState->getRobotPos( robot->getRobotID() ); bool haveBall = false; double angleToTarget = 0; double threshold = 0.017; if(this->predicates & Task::pass){ bool canPass=this->evaluationModule.checkAngleToPass(targetPosition, currRobotPose, angleToTarget); threshold = 0.017 * currRobotPose.getPosition().distance(this->targetPosition); LOG_INFO(this->log,"set threshold to "<<threshold); if( canPass ){ this->robot->stop(); LOG_INFO(log,"Rotation to pass OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget); return Task::ok; } } else{ angleToTarget = calculateAngleToTarget( currRobotPose, Pose(targetPosition.x,targetPosition.y,0) ); double threshold = 0.017; //1 stopien if( fabs( angleToTarget ) < threshold ){ this->robot->stop(); LOG_INFO(log,"Rotation OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget); return Task::ok; } } double w = robot->calculateAngularVel( currRobotPose, targetPosition, currGameState->getSimTime(), haveBall ); LOG_INFO(log,"move robot from"<<currRobotPose<<" to "<<targetPosition<<" setVel w "<<w<<" angle to target "<<angleToTarget); robot->setRelativeSpeed( Vector2D(0,0), w ); } usleep(100); } if(this->stopTask) return Task::ok; return Task::not_completed; }
bool PoseOptimizationObjectiveFunction::getLocalHessian(numopt_common::SparseMatrix& hessian, const numopt_common::Parameterization& params, bool newParams) { // Numerical approach. // numopt_common::SparseMatrix numericalHessian(params.getLocalSize(), params.getLocalSize()); // NonlinearObjectiveFunction::estimateLocalHessian(numericalHessian, params, 1.0e-6); // std::cout << "Numerical:\n" << numericalHessian << std::endl; // Analytical approach. Eigen::MatrixXd analyticalHessian(params.getLocalSize(), params.getLocalSize()); analyticalHessian.setZero(); const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params); const Pose pose = poseParameterization.getPose(); const Eigen::Vector3d& p = pose.getPosition().vector(); const Eigen::Matrix3d p_skew = kindr::getSkewMatrixFromVector(p); const RotationQuaternion Phi(pose.getRotation()); // Default leg position. for (const auto& footPosition : stance_) { const Eigen::Vector3d& f_i = footPosition.second.vector(); const Eigen::Matrix3d f_i_skew = kindr::getSkewMatrixFromVector(f_i); const Eigen::Vector3d Phi_d_i = pose.getRotation().rotate(nominalStanceInBaseFrame_.at(footPosition.first)).vector(); const Eigen::Matrix3d Phi_d_i_skew = kindr::getSkewMatrixFromVector(Phi_d_i); analyticalHessian.topLeftCorner(3, 3) += Eigen::Matrix3d::Identity(); analyticalHessian.topRightCorner(3, 3) += -Phi_d_i_skew; analyticalHessian.bottomLeftCorner(3, 3) += Phi_d_i_skew; analyticalHessian.bottomRightCorner(3, 3) += 0.5 * (p_skew * Phi_d_i_skew + Phi_d_i_skew * p_skew -f_i_skew * Phi_d_i_skew - Phi_d_i_skew * f_i_skew); } // Center of mass. const Eigen::Vector3d p_bar(p.x(), p.y(), 0.0); // Projection. Eigen::Vector3d Phi_r_com = pose.getRotation().rotate(centerOfMassInBaseFrame_).vector(); Phi_r_com.z() = 0.0; const Eigen::Matrix3d Phi_r_com_skew = kindr::getSkewMatrixFromVector(Phi_r_com); const grid_map::Position supportPolygonCentroid(supportRegion_.getCentroid()); const Eigen::Vector3d r_centroid(supportPolygonCentroid.x(), supportPolygonCentroid.y(), 0.0); const Eigen::Matrix3d r_centroid_skew = kindr::getSkewMatrixFromVector(r_centroid); analyticalHessian.topLeftCorner(3, 3) += comWeight_ * Eigen::Vector3d(1.0, 1.0, 0.0).asDiagonal(); analyticalHessian.topRightCorner(3, 3) += -comWeight_ * Phi_r_com_skew; analyticalHessian.bottomLeftCorner(3, 3) += comWeight_ * Phi_r_com_skew; analyticalHessian.bottomRightCorner(3, 3) += 0.5 * comWeight_ * (p_skew * Phi_r_com_skew + Phi_r_com_skew * p_skew -r_centroid_skew * Phi_r_com_skew - Phi_r_com_skew * r_centroid_skew); // Factorized with 2.0 (not weight!). analyticalHessian = 2.0 * analyticalHessian; // std::cout << "Analytical:\n" << analyticalHessian << std::endl << std::endl; // Return solution. // hessian = numericalHessian; hessian = analyticalHessian.sparseView(1e-10); return true; }
geometry_msgs::Pose MessageConversions::poseToMessage(const Pose& pose) const { geometry_msgs::Pose message; const Eigen::Vector3d& position = pose.getPosition(); message.position.x = position[0]; message.position.y = position[1]; message.position.z = position[2]; const Eigen::Quaterniond& orientation = pose.getOrientation(); message.orientation.x = orientation.x(); message.orientation.y = orientation.y(); message.orientation.z = orientation.z(); message.orientation.w = orientation.w(); return message; }
bool PoseOptimizationObjectiveFunction::getLocalGradient(numopt_common::Vector& gradient, const numopt_common::Parameterization& params, bool newParams) { // Numercical approach. // numopt_common::Vector numericalGradient(params.getLocalSize()); // NonlinearObjectiveFunction::estimateLocalGradient(numericalGradient, params, 1.0e-6); // std::cout << "Numerical: " << numericalGradient.transpose() << std::endl; // Analytical approach. numopt_common::Vector analyticalGradient(params.getLocalSize()); analyticalGradient.setZero(); const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params); const Pose pose = poseParameterization.getPose(); const Eigen::Vector3d& p = pose.getPosition().vector(); const RotationQuaternion Phi(pose.getRotation()); // Default leg position. for (const auto& footPosition : stance_) { const Eigen::Vector3d& f_i = footPosition.second.vector(); const Eigen::Vector3d Phi_d_i = Phi.rotate(nominalStanceInBaseFrame_.at(footPosition.first)).vector(); const Eigen::Matrix3d Phi_d_i_skew = kindr::getSkewMatrixFromVector(Phi_d_i); analyticalGradient.head(3) += p + Phi_d_i - f_i; analyticalGradient.tail(3) += Phi_d_i_skew * p - Phi_d_i_skew * f_i; } // Center of mass. const Eigen::Vector3d p_bar(p.x(), p.y(), 0.0); // Projection. Eigen::Vector3d Phi_r_com = Phi.rotate(centerOfMassInBaseFrame_).vector(); Phi_r_com.z() = 0.0; const Eigen::Matrix3d Phi_r_com_skew = kindr::getSkewMatrixFromVector(Phi_r_com); const grid_map::Position supportPolygonCentroid(supportRegion_.getCentroid()); const Eigen::Vector3d r_centroid(supportPolygonCentroid.x(), supportPolygonCentroid.y(), 0.0); analyticalGradient.head(3) += comWeight_ * (p_bar - r_centroid + Phi_r_com); analyticalGradient.tail(3) += comWeight_ * (Phi_r_com_skew * p_bar - Phi_r_com_skew * r_centroid); // Factorized with 2.0 (not weight!). analyticalGradient = 2.0 * analyticalGradient; // std::cout << "Analytical: " << analyticalGradient.transpose() << std::endl << std::endl; // Return solution. // gradient = numericalGradient; gradient = analyticalGradient; return true; }
void FollowLine::execute(void *){ LOG_INFO(log,"create start FollowLine tactic. Line "<<p1<<" "<<p2); Task::status taskStatus = Task::not_completed; Pose goalPose = Pose(this->p1,0); Pose nextGoalPose = Pose(this->p2,0); //znajdz punkt na odcinku (p1,p2) bedacy najblizej pilki //i nie nalezacy do zadnej przeszkody //r-nie prostej przechodzacej przez P1 i P2 // double a = -( p2.y - p1.y )/( p2.x - p1.x ); // double b = 1; // double c = (-1)*p1.x * a - p1.y; GameStatePtr gameState ( new GameState() ); if( Videoserver::getInstance().updateGameState( gameState ) < 0 ) throw SimulationException("FollowLine::execute"); while(true){ { LockGuard l(this->mutex); if(this->stop) break; } taskStatus = Task::not_completed; if( Videoserver::getInstance().updateGameState( gameState ) < 0) throw SimulationException("FollowLine::execute"); Pose currPose = gameState->getRobotPos(this->robot.getRobotID()); Vector2D diff = currPose.getPosition() - goalPose.getPosition(); //podazaj do kranca zdefiniowanego odcinka if( ( fabs(diff.x) < 0.05 ) && ( fabs(diff.y) < 0.05 ) ){ Pose tmp = goalPose; goalPose = nextGoalPose; nextGoalPose = tmp; } Vector2D robotCurrentGlobalVel=gameState->getRobotGlobalVelocity( robot.getRobotID() ); //robotNewVel=calculateVelocity( robotCurrentVel, Pose(targetRelPosition.x,targetRelPosition.y,0)); bool haveBall= false; Vector2D robotNewGlobalVel=this->robot.calculateVelocity( robotCurrentGlobalVel, currPose, goalPose, haveBall ); //double w = robot->calculateAngularVel(*currGameState,robot->getRobotID(), goalPose); double w = robot.calculateAngularVel( gameState->getRobotPos( robot.getRobotID() ), goalPose, gameState->getSimTime(), haveBall ); //this->robot.setRelativeSpeed( Vector2D( 1.0,0.0 ) * sgn( diff.x ) ,0 ); this->robot.setGlobalSpeed( robotNewGlobalVel ,w , currPose.get<2>()); /* while( ( fabs(diff.x) > 0.05 ) && ( fabs(diff.y) > 0.05 ) ){ // newTask = this->currentTask->nextTask(); if( diff.x < 0 ) this->robot.setRelativeSpeed( Vector(1.0,0.0),0 ); if(){ this->currentTask = TaskSharedPtr( newTask ); } break; } */ } this->finished = false; }
bool PoseOptimizationObjectiveFunction::computeValue(numopt_common::Scalar& value, const numopt_common::Parameterization& params, bool newParams) { const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params); const Pose pose = poseParameterization.getPose(); value = 0.0; // Cost for default leg positions. for (const auto& footPosition : stance_) { const Position nominalFootPositionInWorld = pose.getPosition() + pose.getRotation().rotate(nominalStanceInBaseFrame_.at(footPosition.first)); // d_i value += (nominalFootPositionInWorld - footPosition.second).squaredNorm(); } // Cost for foot below hip. // for (const auto& foot : stance_) { // const Position footPosition(foot.second); // const Position hipInWorld = pose.getPosition() + pose.getRotation().rotate(positionsBaseToHipInBaseFrame_.at(foot.first)); // value += (footPosition - hipInWorld).vector().head(2).squaredNorm(); // } // // Cost for deviation from initial position. // Vector positionDifference(state.getPositionWorldToBaseInWorldFrame() - initialPose_.getPosition()); // value += 0.1 * positionDifference.vector().squaredNorm(); // Cost for deviation from initial orientation. // RotationQuaternion rotationDifference(pose.getRotation() * initialPose_.getRotation().inverted()); // const double rotationDifferenceNorm = rotationDifference.norm(); // value += 10.0 * rotationDifferenceNorm * rotationDifferenceNorm; // Cost for deviation from horizontal pose. // RotationVector rotationDifference(pose.getRotation()); // rotationDifference.toImplementation().z() = 0.0; // const double rotationDifferenceNorm = rotationDifference.vector().norm(); // value += 1.0 * rotationDifferenceNorm; // Cost for CoM. const Position centerOfMassInWorldFrame = pose.getPosition() + pose.getRotation().rotate(centerOfMassInBaseFrame_); value += comWeight_ * (supportRegion_.getCentroid().head(2) - centerOfMassInWorldFrame.vector().head(2)).squaredNorm(); // // Cost for torques. //// state_.setPoseBaseToWorld(pose); //// adapter_.setInternalDataFromState(state_); // To guide IK. //// //// for (const auto& foot : stance_) { //// const Position footPositionInBase( //// adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(), foot.second)); //// JointPositionsLeg jointPositions; //// const bool success = adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(footPositionInBase, foot.first, jointPositions); //// if (success) state_.setJointPositionsForLimb(foot.first, jointPositions); //// } // //// adapter_.setInternalDataFromState(state_); // //// adapter_. // // for (const auto& foot : stance_) { // const Position footPosition(foot.second); // const Position baseToFootInBase = pose.getRotation().inverseRotate(footPosition - pose.getPosition()); // Eigen::Vector3d jointPositions; // // https://ashwinnarayan.blogspot.ch/2014/07/inverse-kinematics-for-2dof-arm.html // const double l1 = 1.0; // const double l1_2 = l1 * l1; // const double l2 = 1.0; // const double l2_2 = l2 * l2; // // const double x = -baseToFootInBase(2); // const double x2 = x * x; // const double y = baseToFootInBase(0); // const double y2 = y * y; // const double k = (x2 + y2 - l1_2 - l2_2) / (2 * l1 * l2); // jointPositions(0) = atan2(sqrt(1 - k * k), k); // // const double k1 = l1 + l2 * cos(jointPositions(1)); // const double k2 = l2 * sin(jointPositions(1)); // const double gamma = atan2(k2, k1); // jointPositions(1) = atan2(y, x) - gamma; // value += 0.1 * jointPositions.array().sin().matrix().squaredNorm(); //// JointPositionsLeg jointPositions; //// adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(baseToFootInBase, foot.first, jointPositions); //// value += 0.01 * jointPositions.vector().array().sin().matrix().squaredNorm(); // } return true; }
inline static Translation inverseTransform(const Pose & pose, const Translation & position){ return pose.getRotation().inverseRotate((position-pose.getPosition())); }
inline static Translation transform(const Pose & pose, const Translation & position){ return pose.getRotation().rotate(position) + pose.getPosition(); }