PIDOutput wholeBodyPID(NewQPControllerData *pdata, double t, const Ref<const VectorXd> &q, const Ref<const VectorXd> &qd, const Ref<const VectorXd> &q_des, WholeBodyParams *params) {
// Run a PID controller on the whole-body state to produce desired accelerations and reference posture
PIDOutput out;
double dt = 0;
int nq = pdata->r->num_positions;
assert(q.size() == nq);
assert(qd.size() == pdata->r->num_velocities);
assert(q_des.size() == params->integrator.gains.size());
if (nq != pdata->r->num_velocities) {
mexErrMsgTxt("this function will need to be rewritten when num_pos != num_vel");
}
if (pdata->state.t_prev != 0) {
dt = t - pdata->state.t_prev;
}
pdata->state.q_integrator_state = params->integrator.eta * pdata->state.q_integrator_state + params->integrator.gains.cwiseProduct(q_des - q) * dt;
pdata->state.q_integrator_state = pdata->state.q_integrator_state.array().max(-params->integrator.clamps.array());
pdata->state.q_integrator_state = pdata->state.q_integrator_state.array().min(params->integrator.clamps.array());
out.q_ref = q_des + pdata->state.q_integrator_state;
out.q_ref = out.q_ref.array().max((pdata->r->joint_limit_min - params->integrator.clamps).array());
out.q_ref = out.q_ref.array().min((pdata->r->joint_limit_max + params->integrator.clamps).array());
pdata->state.q_integrator_state = pdata->state.q_integrator_state.array().max(-params->integrator.clamps.array());
pdata->state.q_integrator_state = pdata->state.q_integrator_state.array().min(params->integrator.clamps.array());
VectorXd err_q;
err_q.resize(nq);
err_q.head(3) = q_des.head(3) - q.head(3);
for (int j = 3; j < nq; j++) {
err_q(j) = angleDiff(q(j), q_des(j));
}
out.qddot_des = params->Kp.cwiseProduct(err_q) - params->Kd.cwiseProduct(qd);
out.qddot_des = out.qddot_des.array().max(params->qdd_bounds.min.array());
out.qddot_des = out.qddot_des.array().min(params->qdd_bounds.max.array());
return out;
}
float angleDiffDeg(float a, float b)
{
/*float diff = fmod(b - a + 180.f, 360.f);
if(diff < 0.f) diff += 380.f;
return diff - 180.f;*/
return toDegrees(angleDiff(toRadians(a), toRadians(b)));
}
void Car::applyCollision(const CL_LineSegment2f &p_seg)
{
static const float DAMAGE_MULT = 0.2f;
const float side = -p_seg.point_right_of_line(m_impl->m_position);
const CL_Vec2f segVec = p_seg.q - p_seg.p;
// need front normal (crash side)
CL_Vec2f fnormal(segVec.y, -segVec.x); // right side normal
fnormal.normalize();
if (side < 0) {
fnormal *= -1;
}
// move away
m_impl->m_position += (fnormal * fabs(m_impl->m_speed));
// calculate collision angle to estaminate speed reduction
CL_Angle angleDiff(m_impl->m_phyMoveRot - m_impl->vecToAngle(fnormal));
Workarounds::clAngleNormalize180(&angleDiff);
const float colAngleDeg = fabs(angleDiff.to_degrees()) - 90.0f;
const float reduction = fabs(1.0f - fabs(colAngleDeg - 90.0f) / 90.0f);
// calculate and apply damage
const float damage = m_impl->m_speed * reduction * DAMAGE_MULT;
m_impl->m_damage =
Math::Float::reduce(m_impl->m_damage + damage, 0.0f, 1.0f);
cl_log_event(LOG_DEBUG, "damage: %1, total: %2", damage, m_impl->m_damage);
// reduce speed
m_impl->m_speed -= m_impl->m_speed * reduction;
// bounce movement vector and angle away
// get mirror point
if (m_impl->m_phyMoveVec.length() > 0.01f) {
m_impl->m_phyMoveVec.normalize();
const float lengthProj = m_impl->m_phyMoveVec.length() * cos(segVec.angle(m_impl->m_phyMoveVec).to_radians());
const CL_Vec2f mirrorPoint(segVec * (lengthProj / segVec.length()));
// invert move vector by mirror point
const CL_Vec2f mirrorVec = (m_impl->m_phyMoveVec - mirrorPoint) * -1;
m_impl->m_phyMoveVec = mirrorPoint + mirrorVec;
// update physics angle
m_impl->m_phyMoveRot = m_impl->vecToAngle(m_impl->m_phyMoveVec);
}
}
void SpaceObjectShip :: update(qint64 time)
{
if (mTargetX != -1 && mTargetY != -1)
{
double tAngle = angle(mCoordX, mCoordY, mTargetX, mTargetY);
double dist = distance(mCoordX, mCoordY, mTargetX, mTargetY) / 4;
double diff = angleDiff(mRotation, tAngle);
mDirection = turnDirection(mRotation, tAngle);
if (diff > -dist && diff < dist)
mTrust = true;
else
mTrust = false;
if (dist < 2)
{
mTargetX = -1;
mTargetY = -1;
mTrust = false;
mDirection = 0;
}
}
/* normal calculation of rotation / thrust */
mRotation += mDirection * (time / 1000.0) * ROT_PER_SEC;
if (mRotation < 0) mRotation += 360;
if (mRotation >= 360) mRotation -= 360;
if (mTrust)
{
mCoordX += qCos( DEG2RAD(mRotation) ) * (time / 1000.0) * SPEED_PER_SEC;
mCoordY += qSin( DEG2RAD(mRotation) ) * (time / 1000.0) * SPEED_PER_SEC;
}
}
Vector6d bodyMotionPD(RigidBodyManipulator *r, Map<VectorXd> &q, Map<VectorXd> &qd, const int body_index, const Ref<const Vector6d> &body_pose_des, const Ref<const Vector6d> &body_v_des, const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp, const Ref<const Vector6d> &Kd) {
r->doKinematics(q,false,qd);
// TODO: this must be updated to use quaternions/spatial velocity
Vector6d body_pose;
MatrixXd J = MatrixXd::Zero(6,r->num_positions);
Vector4d zero = Vector4d::Zero();
zero(3) = 1.0;
r->forwardKin(body_index,zero,1,body_pose);
r->forwardJac(body_index,zero,1,J);
Vector6d body_error;
body_error.head<3>()= body_pose_des.head<3>()-body_pose.head<3>();
Vector3d error_rpy,pose_rpy,des_rpy;
pose_rpy = body_pose.tail<3>();
des_rpy = body_pose_des.tail<3>();
angleDiff(pose_rpy,des_rpy,error_rpy);
body_error.tail(3) = error_rpy;
Vector6d body_vdot = (Kp.array()*body_error.array()).matrix() + (Kd.array()*(body_v_des-J*qd).array()).matrix() + body_vdot_des;
return body_vdot;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs<1) mexErrMsgTxt("usage: ptr = pelvisMotionControlmex(0,robot_obj,alpha,nominal_pelvis_height,Kp,Kd); y=pelvisMotionControlmex(ptr,x)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct PelvisMotionControlData* pdata;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct PelvisMotionControlData;
// get robot mex model ptr
if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the second argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r));
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the third argument should be alpha");
memcpy(&(pdata->alpha),mxGetPr(prhs[2]),sizeof(pdata->alpha));
if (!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the fourth argument should be nominal_pelvis_height");
memcpy(&(pdata->nominal_pelvis_height),mxGetPr(prhs[3]),sizeof(pdata->nominal_pelvis_height));
if (!mxIsNumeric(prhs[4]) || mxGetM(prhs[4])!=6 || mxGetN(prhs[4])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the fifth argument should be Kp");
memcpy(&(pdata->Kp),mxGetPr(prhs[4]),sizeof(pdata->Kp));
if (!mxIsNumeric(prhs[5]) || mxGetM(prhs[5])!=6 || mxGetN(prhs[5])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the sixth argument should be Kd");
memcpy(&(pdata->Kd),mxGetPr(prhs[5]),sizeof(pdata->Kd));
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:pelvisMotionControlmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
pdata->pelvis_height_previous = -1;
pdata->pelvis_body_index = pdata->r->findLinkInd("pelvis", 0);
pdata->rfoot_body_index = pdata->r->findLinkInd("r_foot", 0);
pdata->lfoot_body_index = pdata->r->findLinkInd("l_foot", 0);
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
int nq = pdata->r->num_dof;
int narg = 1;
double *q = mxGetPr(prhs[narg++]);
double *qd = &q[nq];
Map<VectorXd> qdvec(qd,nq);
double lfoot_yaw = mxGetScalar(prhs[narg++]);
double rfoot_yaw = mxGetScalar(prhs[narg++]);
pdata->r->doKinematics(q,false,qd);
// TODO: this must be updated to use quaternions/spatial velocity
Vector6d pelvis_pose,rfoot_pose,lfoot_pose;
MatrixXd Jpelvis = MatrixXd::Zero(6,pdata->r->num_dof);
Vector4d zero = Vector4d::Zero();
zero(3) = 1.0;
pdata->r->forwardKin(pdata->pelvis_body_index,zero,1,pelvis_pose);
pdata->r->forwardJac(pdata->pelvis_body_index,zero,1,Jpelvis);
pdata->r->forwardKin(pdata->rfoot_body_index,zero,1,rfoot_pose);
pdata->r->forwardKin(pdata->lfoot_body_index,zero,1,lfoot_pose);
if (pdata->pelvis_height_previous<0) {
pdata->pelvis_height_previous = pelvis_pose(2);
}
double min_foot_z = std::min(lfoot_pose(2),rfoot_pose(2));
double mean_foot_yaw = angleAverage(lfoot_yaw,rfoot_yaw);
double pelvis_height_desired = pdata->alpha*pdata->pelvis_height_previous + (1.0-pdata->alpha)*(min_foot_z + pdata->nominal_pelvis_height);
pdata->pelvis_height_previous = pelvis_height_desired;
Vector6d body_des;
double nan = std::numeric_limits<double>::quiet_NaN();
body_des << nan,nan,pelvis_height_desired,0,0,mean_foot_yaw;
Vector6d error;
error.head<3>()= body_des.head<3>()-pelvis_pose.head<3>();
Vector3d error_rpy,pose_rpy,des_rpy;
pose_rpy = pelvis_pose.tail<3>();
des_rpy = body_des.tail<3>();
angleDiff(pose_rpy,des_rpy,error_rpy);
error.tail(3) = error_rpy;
Vector6d body_vdot = (pdata->Kp.array()*error.array()).matrix() - (pdata->Kd.array()*(Jpelvis*qdvec).array()).matrix();
plhs[0] = eigenToMatlab(body_vdot);
}
void ADynamicCarController::Tick(float DeltaSeconds)
{
Super::Tick(DeltaSeconds);
if (play) {
float deltaSec = GWorld->GetWorld()->GetDeltaSeconds();
updateTarget();
if (first) {
first = false;
FRotator rotation = agent->GetActorRotation();
rotation.Yaw = getRotation(agent->GetActorLocation(), target);
agent->SetActorRotation(rotation);
if (followPath) {
waypoints = DubinsPath::getPath(waypoints, to2D(agent->GetActorLocation()), rotation.Yaw, maxAngle, L, graph, errorTolerance);
writeWaypointsToFile("Waypoints2.txt");
if (waypoints.Num() > 0) {
target = waypoints[0];
}
}
}
if (waypointReached()) {
bool t35 = followPath && waypointsIndex >= waypoints.Num();
bool t4 = avoidAgents && !followPath;
if (t35 || t4) {
play = false;
GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Green, FString::Printf(TEXT("Time: %f\r\n"), totalTime));
}
return;
}
float a = getAcceleration(deltaSec);
v += a;
v = UKismetMathLibrary::FClamp(v, -vMax, vMax);
float rotation = rotate(deltaSec);
FVector vPref = FVector(v * UKismetMathLibrary::DegCos(rotation) * deltaSec, v * UKismetMathLibrary::DegSin(rotation) * deltaSec, 0);
vPref = vPref.GetClampedToMaxSize2D(UKismetMathLibrary::FMin(v * deltaSec, FVector2D::Distance(to2D(agent->GetActorLocation()), target)));
FVector oldVel = velocity;
if (avoidAgents) {
adjustVelocity(to2D(vPref), deltaSec);
//velocity = vPref;
} else {
velocity = vPref;
}
FVector2D q = to2D(velocity - oldVel);
float dv = FMath::Sqrt(FVector2D::DotProduct(q, q));
if (dv > aMax * deltaSec) {
float f = aMax * deltaSec / dv;
velocity = (1 - f) * oldVel + f * velocity;
}
float rot = agent->GetActorRotation().Yaw;
if (velocity.Size2D() != 0) {
if (angleDiff(rot, velocity.Rotation().Yaw) > 90) {
velocity = -velocity;
setRotation();
velocity = -velocity;
} else {
setRotation();
}
}
/*
if (UKismetMathLibrary::Abs(angleDiff(rot, agent->GetActorRotation().Yaw)) / deltaSec > maxAngle * (v / L) + 5) {
GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Red, FString::Printf(TEXT("rot: %f yaw: %f -> %f (%f) %d %s -> %s"), rot, agent->GetActorRotation().Yaw, angleDiff(rot, agent->GetActorRotation().Yaw), UKismetMathLibrary::Abs(angleDiff(rot, agent->GetActorRotation().Yaw)) / deltaSec, vPref.Equals(velocity), *to2D(vPref).ToString(), *to2D(velocity).ToString()));
DrawDebugLine(GWorld->GetWorld(), FVector(to2D(agent->GetActorLocation()), 20), FVector(to2D(agent->GetActorLocation()), 30), FColor::Red, false, 0.5, 0, 1);
GEngine->DeferredCommands.Add(TEXT("pause"));
}*/
agent->SetActorLocation(agent->GetActorLocation() + velocity);
/*
DrawDebugLine(GWorld->GetWorld(), to3D(target), to3D(target) + collisionSize, collisionColor, false, 0.1, 0, 1);
float a = getAcceleration(deltaSec);
v += a;
v = UKismetMathLibrary::FClamp(v, -vMax, vMax);
float rotation = rotate(deltaSec);
acceleration.X = a * UKismetMathLibrary::DegCos(rotation);
acceleration.Y = a * UKismetMathLibrary::DegSin(rotation);
drawLine(2 * acceleration / deltaSec, accelerationColor);
velocity = FVector(v * UKismetMathLibrary::DegCos(rotation) * deltaSec, v * UKismetMathLibrary::DegSin(rotation) * deltaSec, 0);
float rot = agent->GetActorRotation().Yaw;
setRotation();
GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Red, FString::Printf(TEXT("%f"), UKismetMathLibrary::Abs(rot - agent->GetActorRotation().Yaw) / deltaSec));
agent->SetActorLocation(agent->GetActorLocation() + velocity);
DrawDebugLine(GWorld->GetWorld(), agent->GetActorLocation(), agent->GetActorLocation() + collisionSize, FColor::Green, false, 0.1, 0, 1);
*/
}
}
inline double unwrap(double previousAngle, double newAngle) {
return previousAngle - angleDiff(newAngle, angleConv(previousAngle));
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs<1) mexErrMsgTxt("usage: ptr = bodyMotionControlmex(0,robot_obj,Kp,Kd,body_index); y=bodyMotionControlmex(ptr,x,body_pose_des,body_v_des,body_vdot_des)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct BodyMotionControlData* pdata;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct BodyMotionControlData;
// get robot mex model ptr
if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the second argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r));
if (!mxIsNumeric(prhs[2]) || mxGetM(prhs[2])!=6 || mxGetN(prhs[2])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the third argument should be Kp");
memcpy(&(pdata->Kp),mxGetPrSafe(prhs[2]),sizeof(pdata->Kp));
if (!mxIsNumeric(prhs[3]) || mxGetM(prhs[3])!=6 || mxGetN(prhs[3])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the fourth argument should be Kd");
memcpy(&(pdata->Kd),mxGetPrSafe(prhs[3]),sizeof(pdata->Kd));
pdata->body_index = (int) mxGetScalar(prhs[4]) -1;
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:bodyMotionControlmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
int nq = pdata->r->num_positions;
int nv = pdata->r->num_velocities;
int narg = 1;
Map<VectorXd> q(mxGetPrSafe(prhs[narg]), nq);
Map<VectorXd> qd(mxGetPrSafe(prhs[narg++]) + nq, nv);
assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1);
Map< Vector6d > body_pose_des(mxGetPrSafe(prhs[narg++]));
assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1);
Map< Vector6d > body_v_des(mxGetPrSafe(prhs[narg++]));
assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1);
Map< Vector6d > body_vdot_des(mxGetPrSafe(prhs[narg++]));
pdata->r->doKinematics(q, qd);
// TODO: this must be updated to use quaternions/spatial velocity
auto body_pose_gradientvar = pdata->r->forwardKin(Vector3d::Zero().eval(), pdata->body_index, 0, 1, 1);
const auto& body_pose = body_pose_gradientvar.value();
const auto& J = body_pose_gradientvar.gradient().value();
Vector6d body_error;
body_error.head<3>()= body_pose_des.head<3>()-body_pose.head<3>();
Vector3d error_rpy,pose_rpy,des_rpy;
pose_rpy = body_pose.tail<3>();
des_rpy = body_pose_des.tail<3>();
angleDiff(pose_rpy,des_rpy,error_rpy);
body_error.tail(3) = error_rpy;
Vector6d body_vdot = (pdata->Kp.array()*body_error.array()).matrix() + (pdata->Kd.array()*(body_v_des-J*qd).array()).matrix() + body_vdot_des;
plhs[0] = eigenToMatlab(body_vdot);
}
void localizerGVG::handleOdom(const nav_msgs::Odometry::ConstPtr& odom) {
double x=odom->pose.pose.position.x;
double y=odom->pose.pose.position.y;
tf::Pose pose;
tf::poseMsgToTF(odom->pose.pose, pose);
double yaw=tf::getYaw(pose.getRotation());
ROS_DEBUG("IN [x=%f, y=%f, yaw=%f]",x,y,yaw);
if(std::isnan(yaw) || std::isnan(x) || std::isnan(y)){
ROS_WARN("Odom read nan, skipping iteration of publishing and hoping for recovery.");
return;
}
if(!initOdom) {
oldX=x;
oldY=y;
oldYaw=yaw;
initOdom=true;
oldStamp=odom->header.stamp;
Vector3d odomVector(x, y, yaw);
X.segment(0, 3) = odomVector;
return;
}
if(this->filterOn) {
if((oldX==x)&&(oldY==y)&&(oldYaw==yaw)){
oldStamp=odom->header.stamp;
return;
}
// Calculate the measurement
double dx=x-oldX;
double dy=y-oldY;
double dYaw=angleDiff(yaw,oldYaw);
double dt=odom->header.stamp.toSec()-oldStamp.toSec();
if(dt==0) {
ROS_DEBUG("dt in localizer equal to zero, will lead to nan. skipping and hoping for recovery");
return;
}
double Vm=sqrt(dx*dx+dy*dy)/dt;
double Wm=dYaw/dt;
X(0)=X(0)+Vm*dt*cos(X(2));
X(1)=X(1)+Vm*dt*sin(X(2));
X(2)=thetapp(X(2)+Wm*dt);
propagate(Vm,Wm,dt);
// Publish the odometry with covariance topic
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(X(2));
nav_msgs::Odometry outputPose;
outputPose.header.frame_id = "odom";
outputPose.header.stamp =odom->header.stamp;
outputPose.pose.pose.position.x = X(0);
outputPose.pose.pose.position.y = X(1);
outputPose.pose.pose.position.z = 0;
outputPose.pose.pose.orientation=odom_quat;
if(std::isnan(X(0)),std::isnan(X(1)))
{
ROS_WARN("Filter made something nan, skipping publishing and hoping for recovery.");
return;
}
for (unsigned int i=0; i<2; i++)
for (unsigned int j=0; j<2; j++)
outputPose.pose.covariance[6*i+j] = P(i,j);
posePub.publish(outputPose);
}
oldX=x;
oldY=y;
oldYaw=yaw;
oldStamp=odom->header.stamp;
}
bool localizerGVG::processMeetpoint(localizer::UpdateFilter::Request &req,
localizer::UpdateFilter::Response &res) {
ROS_INFO("processMeetpoint received meetpoint %d [%lf,%lf,%lf]", req.id, req.x, req.y,req.yaw);
ROS_INFO("processMeetpoint robot pose [%lf,%lf,%lf]",X(0),X(1),X(2));
if(nL==0) {
//Reset the odometry covariance.
P=MatrixXd::Zero(3,3);
}
// Propagate the cross-corelation between robot and the
// existing landmarks up to now
propagateRL();
// For the new landmark
Vector3d xl; //the coordinates of the landmark (meetpoint) in World Frame
Vector3d z; // Vector representation of the measurment
z<<req.x, req.y, req.yaw;
xl.segment(0,2)=X.segment(0,2)+C(X(2))*z.segment(0,2);
xl(2)=X(2)+z(2);
// Create the measurment matrices
MatrixXd Hr(3,3);
Vector2d dx;
dx=xl.segment(0,2)-X.segment(0,2);
double _c=cos(X(2));
double _s=sin(X(2));
Hr<<-_c, -_s, + _c*dx(1)-_s*dx(0),
_s, -_c, - _s*dx(1)-_c*dx(0),
0, 0, -1;
Matrix3d Hl; // Hl is a 2 by 2 Matrix
Hl=MatrixXd::Zero(3,3);
Hl.block(0,0,2,2)=CT(X(2));
Hl(2,2)=1;
if(newLandmark(req.id)) {
// If pre-fix on loaded map, need to mark that this new node is in current frame
if (!mapLoadLocalization) preFixIdList.push_back(req.id);
// New Landmark
idList.push_back(req.id);
P.conservativeResize(P.rows()+3,P.cols()+3);
X.conservativeResize(X.size()+3);
X.segment(3+nL*3,3)=xl;
// Create new Pllcv matrices describing the cross corelation
// between the old landmarks and the new Landmark. Page 79 eq. 5.73
for (int count = 0; count < nL; count++) {
Matrix3d newPllcv;
newPllcv = -P.block(3 + count*3, 0, 3, 3)*Hr.transpose()*Hl;
P.block(3 + count*3, 3 + nL*3, 3, 3) = newPllcv;
P.block(3 + nL*3, 3 + count*3, 3, 3) = newPllcv.transpose();
}
// Create new Prl matrix describing the cross corelation
// between the Robot and the new Landmark. Page 79 eq. 5.73
MatrixXd newPrl(3,3);
newPrl=-P.block(0,0,3,3)*Hr.transpose()*Hl;
P.block(0,3+nL*3,3,3)=newPrl;
P.block(3+nL*3,0,3,3)=newPrl.transpose();
// Create the new Pll matrix describing the covariance of the landmark
// page 79 eq. 5.74
Matrix3d newPll;
newPll=Hl.transpose()*(Hr*P.block(0,0,3,3)*Hr.transpose()+R)*Hl;
P.block(3+nL*3,3+nL*3,3,3)=newPll;
nL++;
}
else {
// Find which landmark it is:
int id=-1;
for(unsigned int i=0; i<idList.size(); i++) {
if(idList[i] == req.id) id=i;
}
//Sanity check:
if(id < 0) {
cerr<<"Not able to find id:"<<req.id<< "in the list of ids"<<endl;
for(unsigned int i=0; i<idList.size(); i++){
cerr<<idList[i]<<" ";
}
cerr<<endl;
}
// Check if the revisited node was one from previous map or current map
// If from current map, don't use this node to compute transformation between old/new map
bool oldMapNode = true;
for (vector<int>::iterator it = preFixIdList.begin(); it != preFixIdList.end(); ++it){
if((*it)==id) {
oldMapNode = false;
break;
}
}
if (!mapLoadLocalization && oldMapNode) {
// Copy the covariance information to Prr and also Xr
Vector3d offset;
offset(2) = X(2)-bearing_angle;
Vector2d v;
v(0) = X(3*id+3);
v(1) = X(3*id+4);
Vector2d rotated_mp;
rotated_mp = C(offset(2))*v;
offset(0) = X(0)-rotated_mp(0);
offset(1) = X(1)-rotated_mp(1);
/*P.block(0,0,3,3) = P_init.block(3*id+3, 3*id+3, 3, 3);
// Copying information to Prl
P.block(0,3,3,3*nL) = P.block(3*id+3, 3, 3, 3*nL);
P.block(3,0,3*nL,3) = P.block(3, 3*id+3, 3*nL, 3);*/
mapLoadLocalization = true;
// Calculate translation between current frame and loaded map frame
for (unsigned int i = 0; i < nL; i++) {
bool corrected = false;
for (vector<int>::iterator it = preFixIdList.begin(); it != preFixIdList.end(); ++it){
if((*it)==i) {
corrected = true;
break;
}
}
if (!corrected) {
Vector2d v;
v(0) = X(3*i+3);
v(1) = X(3*i+4);
Vector2d result;
result = C(offset(2))*v;
X(3*i+3) = result(0);
X(3*i+4) = result(1);
X(3*i+3) = X(3*i+3) + offset(0);
X(3*i+4) = X(3*i+4) + offset(1);
X(3*i+5) = thetapp(X(3*i+5) + offset(2));
}
}
P = P_init;
X = X_init;
}
else {
// perform update
ROS_INFO("localizerGVG::processMeetpoint id %d %d",req.id, id);
Vector3d z_est;
z_est.segment(0,2)=CT(X(2))*(X.segment(3+3*id,2)-X.segment(0,2));
z_est(2)=X(3+3*id+2)-X(2);
Vector3d r;
r.segment(0,2)=z.segment(0,2)-z_est.segment(0,2);
r(2)=angleDiff(z(2),z_est(2));
MatrixXd H(3,3+3*nL);
H=MatrixXd::Zero(3,3+3*nL);
H.block(0,0,3,3)=Hr;
H.block(0,3+3*id,3,3)=Hl;
MatrixXd S(3,3);
S=H*P*H.transpose()+R;
MatrixXd K(3,3+3*nL);
K=P*H.transpose()*S.inverse();
X=X+K*r;
X(2)=thetapp(X(2));
P=(MatrixXd::Identity(3+nL*3,3+nL*3)-K*H)*P*(MatrixXd::Identity(3+nL*3,3+nL*3)-K*H).transpose()+K*R*K.transpose();
}
}
// publish the odometry with covariance topic
// Publish the Map
localizer::GVGmap theMap;
for(unsigned int i=0; i<X.size(); i++) {
theMap.state.push_back(X[i]);
res.X.push_back(X[i]);
}
for(unsigned int i=0; i<P.rows(); i++) {
for(unsigned int j=0; j<P.cols(); j++) {
theMap.cov.push_back(P(i,j));
res.P.push_back(P(i,j));
}
}
mapPub.publish(theMap);
return(true);
}
void hsm_match(struct hsm_params*p, hsm_buffer b1, hsm_buffer b2) {
sm_log_push("hsm_match");
/* Let's measure the time */
clock_t hsm_match_start = clock();
assert(b1->num_angular_cells == b2->num_angular_cells);
assert(p->max_translation > 0);
assert(b1->linear_cell_size > 0);
b1->num_valid_results = 0;
/* Compute cross-correlation of spectra */
hsm_circular_cross_corr_stupid(b1->num_angular_cells, b2->hs, b1->hs, b1->hs_cross_corr);
/* Find peaks in cross-correlation */
int peaks[p->num_angular_hypotheses], npeaks;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->angular_hyp_min_distance_deg, 0, p->num_angular_hypotheses, peaks, &npeaks);
sm_debug("Found %d peaks (max %d) in cross correlation.\n", npeaks, p->num_angular_hypotheses);
if(npeaks == 0) {
sm_error("Cross correlation of spectra has 0 peaks.\n");
sm_log_pop();
return;
}
sm_log_push("loop on theta hypotheses");
/* lag e' quanto 2 si sposta a destra rispetto a 1 */
for(int np=0;np<npeaks;np++) {
int lag = peaks[np];
double theta_hypothesis = lag * (2*M_PI/b1->num_angular_cells);
sm_debug("Theta hyp#%d: lag %d, angle %fdeg\n", np, lag, rad2deg(theta_hypothesis));
/* Superimpose the two spectra */
double mult[b1->num_angular_cells];
for(int r=0;r<b1->num_angular_cells;r++)
mult[r] = b1->hs[r] * b2->hs[pos_mod(r-lag, b1->num_angular_cells)];
/* Find directions where both are intense */
int directions[p->xc_ndirections], ndirections;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->xc_directions_min_distance_deg, 1, p->xc_ndirections, directions, &ndirections);
if(ndirections<2) {
sm_error("Too few directions.\n");
}
struct {
/* Direction of cross correlation */
double angle;
int nhypotheses;
struct {
double delta;
double value;
} hypotheses[p->linear_xc_max_npeaks];
} dirs[ndirections];
sm_debug("Using %d (max %d) correlations directions.\n", ndirections, p->xc_ndirections);
int max_lag = (int) ceil(p->max_translation / b1->linear_cell_size);
int min_lag = -max_lag;
sm_debug("Max lag: %d cells (max t: %f, cell size: %f)\n",
max_lag, p->max_translation, b1->linear_cell_size);
sm_log_push("loop on xc direction");
/* For each correlation direction */
for(int cd=0;cd<ndirections;cd++) {
dirs[cd].angle = theta_hypothesis + (directions[cd]) * (2*M_PI/b1->num_angular_cells);
printf(" cd %d angle = %d deg\n", cd, (int) rad2deg(dirs[cd].angle));
/* Do correlation */
int lags [2*max_lag + 1];
double xcorr [2*max_lag + 1];
int i1 = pos_mod(directions[cd] , b1->num_angular_cells);
int i2 = pos_mod(directions[cd] + lag , b1->num_angular_cells);
double *f1 = b1->ht[i1];
double *f2 = b2->ht[i2];
hsm_linear_cross_corr_stupid(
b2->num_linear_cells,f2,
b1->num_linear_cells,f1,
xcorr, lags, min_lag, max_lag);
/* Find peaks of cross-correlation */
int linear_peaks[p->linear_xc_max_npeaks], linear_npeaks;
hsm_find_peaks_linear(
2*max_lag + 1, xcorr, p->linear_xc_peaks_min_distance/b1->linear_cell_size,
p->linear_xc_max_npeaks, linear_peaks, &linear_npeaks);
sm_debug("theta hyp #%d: Found %d (max %d) peaks for correlation.\n",
cd, linear_npeaks, p->linear_xc_max_npeaks);
dirs[cd].nhypotheses = linear_npeaks;
sm_log_push("Considering each peak of linear xc");
for(int lp=0;lp<linear_npeaks;lp++) {
int linear_xc_lag = lags[linear_peaks[lp]];
double value = xcorr[linear_peaks[lp]];
double linear_xc_lag_m = linear_xc_lag * b1->linear_cell_size;
sm_debug("lag: %d delta: %f value: %f \n", linear_xc_lag, linear_xc_lag_m, value);
dirs[cd].hypotheses[lp].delta = linear_xc_lag_m;
dirs[cd].hypotheses[lp].value = value;
}
sm_log_pop();
if(p->debug_true_x_valid) {
double true_delta = cos(dirs[cd].angle) * p->debug_true_x[0] +
sin(dirs[cd].angle) * p->debug_true_x[1];
sm_debug("true_x delta = %f \n", true_delta );
}
} /* xc direction */
sm_log_pop();
sm_debug("Now doing all combinations. How many are there?\n");
int possible_choices[ndirections];
int num_combinations = 1;
for(int cd=0;cd<ndirections;cd++) {
possible_choices[cd] = dirs[cd].nhypotheses;
num_combinations *= dirs[cd].nhypotheses;
}
sm_debug("Total: %d combinations\n", num_combinations);
sm_log_push("For each combination..");
for(int comb=0;comb<num_combinations;comb++) {
int choices[ndirections];
hsm_generate_combinations(ndirections, possible_choices, comb, choices);
/* Linear least squares */
double M[2][2]={{0,0},{0,0}}; double Z[2]={0,0};
/* heuristic quality value */
double sum_values = 0;
for(int cd=0;cd<ndirections;cd++) {
double angle = dirs[cd].angle;
double c = cos(angle), s = sin(angle);
double w = dirs[cd].hypotheses[choices[cd]].value;
double y = dirs[cd].hypotheses[choices[cd]].delta;
M[0][0] += c * c * w;
M[1][0] += c * s * w;
M[0][1] += c * s * w;
M[1][1] += s * s * w;
Z[0] += w * c * y;
Z[1] += w * s * y;
sum_values += w;
}
double det = M[0][0]*M[1][1]-M[0][1]*M[1][0];
double Minv[2][2];
Minv[0][0] = M[1][1] * (1/det);
Minv[1][1] = M[0][0] * (1/det);
Minv[0][1] = -M[0][1] * (1/det);
Minv[1][0] = -M[1][0] * (1/det);
double t[2] = {
Minv[0][0]*Z[0] + Minv[0][1]*Z[1],
Minv[1][0]*Z[0] + Minv[1][1]*Z[1]};
/* copy result in results slot */
int k = b1->num_valid_results;
b1->results[k][0] = t[0];
b1->results[k][1] = t[1];
b1->results[k][2] = theta_hypothesis;
b1->results_quality[k] = sum_values;
b1->num_valid_results++;
}
sm_log_pop();
} /* theta hypothesis */
sm_log_pop();
/* for(int i=0;i<b1->num_valid_results;i++) {
printf("#%d %.0fdeg %.1fm %.1fm quality %f \n",i,
rad2deg(b1->results[i][2]),
b1->results[i][0],
b1->results[i][1],
b1->results_quality[i]);
}*/
/* Sorting based on values */
int indexes[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++)
indexes[i] = i;
qsort_descending(indexes, (size_t) b1->num_valid_results, b1->results_quality);
/* copy in the correct order*/
double*results_tmp[b1->num_valid_results];
double results_quality_tmp[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++) {
results_tmp[i] = b1->results[i];
results_quality_tmp[i] = b1->results_quality[i];
}
for(int i=0;i<b1->num_valid_results;i++) {
b1->results[i] = results_tmp[indexes[i]];
b1->results_quality[i] = results_quality_tmp[indexes[i]];
}
for(int i=0;i<b1->num_valid_results;i++) {
char near[256]="";
double *x = b1->results[i];
if(p->debug_true_x_valid) {
double err_th = rad2deg(fabs(angleDiff(p->debug_true_x[2],x[2])));
double err_m = hypot(p->debug_true_x[0]-x[0],
p->debug_true_x[1]-x[1]);
const char * ast = (i == 0) && (err_th > 2) ? " ***** " : "";
sprintf(near, "th err %4d err_m %5f %s",(int)err_th ,err_m,ast);
}
if(i<10)
printf("after #%d %3.1fm %.1fm %3.0fdeg quality %5.0f \t%s\n",i,
x[0],
x[1], rad2deg(x[2]), b1->results_quality[i], near);
}
/* How long did it take? */
clock_t hsm_match_stop = clock();
int ticks = hsm_match_stop-hsm_match_start;
double ctime = ((double)ticks) / CLOCKS_PER_SEC;
sm_debug("Time: %f sec (%d ticks)\n", ctime, ticks);
sm_log_pop();
}
double
GeomHelper::angle2D(const Position& p1, const Position& p2) {
return angleDiff(atan2(p1.y(), p1.x()), atan2(p2.y(), p2.x()));
}
