Exemplo n.º 1
0
	void update(float t, float dt) {
		Eigen::Vector2f dist = position - goal;
		float dist_norm = dist.norm();
		if(dist_norm < 1) {
			set_random_goal();
			return;
		}
		position += dt*dist*SPEED/dist_norm;
	}
Exemplo n.º 2
0
void
Chain::solveJacobian(const float stepSize, 
                     const int maxIterations,
                     const vec2& desiredPos, 
                     std::vector<float>* iterPose)
{
    vec3 G = vec3(desiredPos, 1); 
    vec3 endEffector; worldEndPos(endEffector);
   
    Eigen::Vector2f error;
    error(0) = G.x - endEffector.x;
    error(1) = G.y - endEffector.y;
    
    int iter = 0;
    int linkCount = count();
    while (iter < maxIterations  && error.norm() > EPSILON) { 
        Eigen::Vector2f dx = error * stepSize;
        
        // Calculate jacobian
        Eigen::MatrixXf jacobian(2,linkCount); 
        for (int j = 0; j < linkCount; j++) {
            vec3 jo; worldJointPos(j, jo);
            vec3 w = endEffector - jo;
            vec3 temp = cross(vec3(0,0,1.0f),vec3(w.x, w.y, 1));
            jacobian(0,j) = temp.x;
            jacobian(1,j) = temp.y;
        }
        Eigen::MatrixXf jacobianT = jacobian.transpose();
        Eigen::MatrixXf invJacobian = jacobian * jacobianT;
        invJacobian = jacobianT * invJacobian.inverse(); 
        Eigen::Vector4f delta = invJacobian * dx;
        for (int i = 0; i < linkCount; i++) {
            Link * l = getLink(i);
            if (delta(i) != delta(i)) {
                // Nudge angles arbitrarily to
                // break singularity for next iteration
                l->angle += 1;
            } else {
                l->angle += delta(i); 
            }
            if (iterPose) {
                iterPose->push_back(l->angle);
            }
        }
        
        // Update world frames for all joints and end effector
        worldEndPos(endEffector);    

        error(0) = G.x - endEffector.x;
        error(1) = G.y - endEffector.y;
        iter++;
    }
}
Exemplo n.º 3
0
Eigen::Vector2f QS::Walk::evaluate(const Actor *theActor)
{
  Eigen::Vector2f desiredVelocity(mySpeed_ms, 0.0);
  Eigen::Vector2f velocity = theActor->getVelocity();
  if (velocity.norm() >= mySpeed_ms)
  {
    desiredVelocity << 0.0, 0.0;
  }
  Eigen::Vector2f steeringForce = desiredVelocity *
    theActor->getMass();

  Eigen::Rotation2Df rotation(theActor->getOrientation());
  steeringForce = rotation * steeringForce;
  return steeringForce;
}
Exemplo n.º 4
0
    auto match(vfloat2 p, vfloat2 tr_prediction,
	       F A, F B, GD Ag,
	       const int winsize,
	       const float min_ev_th,
	       const int max_interations,
	       const float convergence_delta)
    {
      typedef typename F::value_type V;
      int WS = winsize;
      int ws = winsize;
      int hws = ws/2;

      // Gradient matrix
      Eigen::Matrix2f G = Eigen::Matrix2f::Zero();
      int cpt = 0;
      for(int r = -hws; r <= hws; r++)
	for(int c = -hws; c <= hws; c++)
	  {
	    vfloat2 n = p + vfloat2(r, c);
	    if (A.has(n.cast<int>()))
	      {
		Eigen::Matrix2f m;
		auto g = Ag.linear_interpolate(n);
		float gx = g[0];
		float gy = g[1];
		m <<
		  gx * gx, gx * gy,
		  gx * gy, gy * gy;
		G += m;
		cpt++;
	      }
	  }

      // Check minimum eigenvalue.
      float min_ev = 99999.f;
      auto ev = (G / cpt).eigenvalues();
      for (int i = 0; i < ev.size(); i++)
	if (fabs(ev[i].real()) < min_ev) min_ev = fabs(ev[i].real());

      if (min_ev < min_ev_th)
	return std::pair<vfloat2, float>(vfloat2(-1,-1), FLT_MAX);

      Eigen::Matrix2f G1 = G.inverse();

      // Precompute gs and as.
      vfloat2 prediction_ = p + tr_prediction;
      vfloat2 v = prediction_;
      Eigen::Vector2f nk = Eigen::Vector2f::Ones();

      char gs_buffer[WS * WS * sizeof(vfloat2)];
      vfloat2* gs = (vfloat2*) gs_buffer;
      // was: vfloat2 gs[WS * WS];

      typedef plus_promotion<V> S;
      char as_buffer[WS * WS * sizeof(S)];
      S* as = (S*) as_buffer;
      // was: S as[WS * WS];
      {
	for(int i = 0, r = -hws; r <= hws; r++)
	  {
	    for(int c = -hws; c <= hws; c++)
	      {
		vfloat2 n = p + vfloat2(r, c);
		if (Ag.has(n.cast<int>()))
		  {
		    gs[i] = Ag.linear_interpolate(n).template cast<float>();
		    as[i] = cast<S>(A.linear_interpolate(n));
		  }
		i++;
	      }
	  }
      }
      auto domain = B.domain();// - border(hws + 1);

      // Gradient descent
      for (int k = 0; k <= max_interations && nk.norm() >= convergence_delta; k++)
	{
	  Eigen::Vector2f bk = Eigen::Vector2f::Zero();
	  // Temporal difference.
	  int i = 0;
	  for(int r = -hws; r <= hws; r++)
	    {
	      for(int c = -hws; c <= hws; c++)          
		{
		  vfloat2 n = p + vfloat2(r, c);
		  if (Ag.has(n.cast<int>()))
		    {
		      vfloat2 n2 = v + vfloat2(r, c);
		      auto g = gs[i];
		      float dt = (cast<float>(as[i]) - cast<float>(B.linear_interpolate(n2)));
		      bk += Eigen::Vector2f{g[0] * dt, g[1] * dt};
		    }
		  i++;
		}
	    }

	  nk = G1 * bk;
	  v += vfloat2{nk[0], nk[1]};

	  if (!domain.has(v.cast<int>()))
	    return std::pair<vfloat2, float>(vfloat2(0, 0), FLT_MAX);
	}

      // Compute the SSD.
      float err = 0;
      for(int r = -hws; r <= hws; r++)
	for(int c = -hws; c <= hws; c++)
	  {
	    vfloat2 n2 = v + vfloat2(r, c);
	    int i = (r+hws) * ws + (c+hws);
	    {
	      err += fabs(cast<float>(as[i] - cast<S>(B.linear_interpolate(n2))));
	      cpt++;
	    }
	  }

      return std::pair<vfloat2, float>(v - p, err / (cpt));


    }
Exemplo n.º 5
0
void derivatives(const StateMatrix& states, StateMatrix& derivs, 
    const ControlMatrix& ctrls, const SimulationParameters& params, 
    const Map & map)
{
  float cd_a_rho = params.linearDrag;  // 0.1  coeff of drag * area * density of fluid
  float k_elastic = params.elasticity;  // 4000.  // spring constant of ships
  float rad = 1.;  // leave radius 1 - we decided to change map scale instead
  const Eigen::VectorXf &masses = params.shipDensities;  // order of 1.0 Mass of ships 
  float spin_drag_ratio = params.rotationalDrag; // 1.8;  // spin friction to translation friction
  float eps = 1e-5;  // Avoid divide by zero special cases
  float mu = params.shipFriction;  // 0.05;  // friction coefficient between ships
  float mu_wall = params.wallFriction;  //0.25*?wallFriction;  // 0.01;  // wall friction parameter
  float wall_restitution = params.wallRestitution; // 0.5
  float ship_restitution = params.shipRestitution; // circa 0.5
  float diameter = 2.*rad;  // rad(i) + rad(j) for any i, j
  float inertia_mass_ratio = 0.25;
  float map_grid = rad * 2. + eps; // must be 2*radius + eps
  std::unordered_map<std::pair<int, int>, std::vector<uint>, 
                     boost::hash<std::pair<int, int>>> bins;

  uint n = states.rows();
  Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2);
  Eigen::VectorXd trq = Eigen::VectorXd::Zero(n);
  // rotationalThrust Order +- 10 
  // linearThrust Order +100
  // mapscale order 10 - thats params.pixelsize
  // Accumulate forces and torques into these:
  uint collide_checks = 0;  // debug count...

  for (uint i=0; i<n; i++) {
    Eigen::Vector2f pos_i;
    pos_i(0) = states(i,0);
    pos_i(1) = states(i,1);
    Eigen::Vector2f vel_i;
    vel_i(0) = states(i,2);
    vel_i(1) = states(i,3);
    float theta_i = states(i,4);
    float w_i = states(i,5);

    // 1. Control 
    float thrusting = ctrls(i, 0);
    float turning = ctrls(i, 1);
    f(i, 0) = thrusting * params.linearThrust * cos(theta_i);
    f(i, 1) = thrusting * params.linearThrust * sin(theta_i);
    trq(i) = turning * params.rotationalThrust;

    // 2. Drag
    f(i, 0) -= cd_a_rho * vel_i(0);
    f(i, 1) -= cd_a_rho * vel_i(1);
    trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i)

    // 3. Inter-ship collisions against ships of lower index...  
    // Figure out this ship's hashes: It has 4 in 2 dimensions
    std::unordered_set<uint> collision_shortlist;
    std::pair<int, int> my_hash;
    for (int dx=-1; dx < 2; dx+=2)
      for (int dy=-1; dy < 2; dy+=2)
      {
        float x_mod = pos_i(0) + float(dx)*rad;
        float y_mod = pos_i(1) + float(dy)*rad;
        my_hash = std::make_pair(int(x_mod / map_grid), 
                                 int(y_mod / map_grid));
        if (bins.count(my_hash) > 0)
        {
            // Already exists - shortlist others and add self
            std::vector<uint> current_bin = bins.find(my_hash)->second; 
            // -->first is the key as it returns a key/value pair
            for (uint bin_idx: current_bin)
              if (bin_idx != i)
                collision_shortlist.insert(bin_idx);
            current_bin.push_back(i);
        }
        else
        {
            // didnt exist - add self, and push into map
            std::vector<uint> current_bin;
            current_bin.push_back(i);
            bins.insert(std::make_pair(my_hash, current_bin));
        }
      }

    for (uint j: collision_shortlist) {  // =i+1; j<n; j++) {
      collide_checks ++;
      // std::cout << "Checking " << i << ", " << j << "\n";
      Eigen::Vector2f pos_j;
      pos_j(0) = states(j,0);
      pos_j(1) = states(j,1);
      Eigen::Vector2f vel_j;
      vel_j(0) = states(j,2);
      vel_j(1) = states(j,3);
      float theta_j = states(j,4);
      float w_j = states(j,5);

      Eigen::Vector2f dP = pos_j - pos_i;
      float dist = dP.norm() + eps - diameter;
      Eigen::Vector2f dPhat = dP / (dP.norm() + eps);
      if (dist < 0) {
        // we have a collision interaction
        
        // A. Direct collision: apply linear spring normal force
        float f_magnitude = - dist * k_elastic; // dist < =
        if ((vel_j - vel_i).dot(pos_j - pos_i) > 0)
            f_magnitude *= ship_restitution;
        Eigen::Vector2f f_norm = f_magnitude * dPhat;
        f(i, 0) -= f_norm(0);
        f(i, 1) -= f_norm(1);
        f(j, 0) += f_norm(0);
        f(j, 1) += f_norm(1);

        // B. Surface frictions: approximate spin effects
        Eigen::Vector2f perp;  // surface tangent pointing +theta direction
        perp(0) = -dPhat(1);
        perp(1) = dPhat(0);
        
        // relative velocities of surfaces
        float v_rel = rad*w_i + rad*w_j + perp.dot(vel_i - vel_j);
        float fric = f_magnitude * mu * sigmoid(v_rel);
        Eigen::Vector2f f_fric = fric * perp;
        f(i, 0) += f_fric(0);
        f(i, 1) += f_fric(1);
        f(j, 0) -= f_fric(0);
        f(j, 1) -= f_fric(1);
        trq(i) -= fric * rad;
        trq(j) -= fric * rad;
      }  // end collision
    } // end loop 3. opposing ship


    // 4. Wall single body collisions
    // compute distance to wall and local normals
    float wall_dist, norm_x, norm_y;
    interpolate_map(pos_i(0), pos_i(1), wall_dist, norm_x, norm_y, map, params);
    float dist = wall_dist - rad;
    if (dist < 0)
    {
      /* if (dist < -1.) */
      /*     assert(false); */

      // Spring force
      float f_norm_mag = -dist*k_elastic;  // dist is negative, f_norm is +ve
      if (norm_x*vel_i(0) + norm_y*vel_i(1) > 0)
          f_norm_mag *= wall_restitution;
      
      if (dist > -rad*0.25)
      {
        // not significantly through wall yet
        f(i, 0) += f_norm_mag * norm_x;
        f(i, 1) += f_norm_mag * norm_y;
      }
      else
      {
        // uh-oh - lets just SET normal forces and seriously damp vel
        f(i, 0) = f_norm_mag * norm_x;
        f(i, 1) = f_norm_mag * norm_y;
        f(i, 0) -= 100. * vel_i(0);
        f(i, 1) -= 100. * vel_i(1);
        
      }
      // Surface friction
      Eigen::Vector2f perp;  // surface tangent pointing +theta direction
      perp(0) = -norm_y;
      perp(1) = norm_x;
      float v_rel = w_i * rad + vel_i(0)*norm_y - vel_i(1)*norm_x;
      float fric = f_norm_mag * mu_wall * sigmoid(v_rel);
      f(i, 0) -= fric*norm_y;
      f(i, 1) += fric*norm_x;
      trq(i) -= fric * rad;
    }
  } // end loop current ship

  // std::cout << "Collision checks:" << collide_checks << "\n";
  // Compose the vector of derivatives:
  float vmax = 40.0;
  for (int i=0; i<n; i++)
  {
    float vx = states(i,2);
    float vy = states(i,3);
    float speed = std::sqrt(vx*vx + vy*vy);
    if (speed > vmax)
    {
      vx *= vmax/speed;
      vy *= vmax/speed;
    }
    
    // x_dot = vx
    derivs(i, 0) = vx;
    // y_dot = vy
    derivs(i, 1) = vy; 
    // vx_dot = fx / m
    float ax = f(i,0)/masses(i);
    float ay = f(i,1)/masses(i);
    
    derivs(i, 2) = ax;
    // vy_dot = fy / m
    derivs(i, 3) = ay;
    // theta_dot = omega
    derivs(i, 4) = states(i, 5);
    // omega_dot = T_r / (inertia_mass_ratio*m)
    derivs(i,5) = trq(i) / (inertia_mass_ratio * masses(i));
  }
  
  // ux uy vx vy  theta omega
  // 0  1  2  3   4     5
}