Jump to content

  • Log In with Google      Sign In   
  • Create Account

Spring phisic and quaternion


Old topic!
Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.

  • You cannot reply to this topic
8 replies to this topic

#1 olkeyn   Members   -  Reputation: 106

Like
0Likes
Like

Posted 31 July 2014 - 12:33 PM

Hi

I am trying to make simulation of mechanical spring affecting on a body fixed by ball joint connection and forcing it to take vertical orientation with torque proportional to angles of deviation from global axes.

I use Open Dinamic Engine for simulation.

Orientation of a body defined with quaternion Q= (w,x,y,z). How can I found angles on wich body rotated to global axes?



Sponsor:

#2 Álvaro   Crossbones+   -  Reputation: 13326

Like
0Likes
Like

Posted 31 July 2014 - 02:02 PM

I am not entirely sure I understand what you are trying to do, but there is a good chance that you don't actually want angles, even if you think you do.

So your body has something like a needle and you want to have some corrective force that tries to make the needle point upwards, right? You can compute a direction in the space of angular velocities that corresponds to the most natural rotation that would make the needle point upwards, and apply a torque proportional to it. Do you think that would satisfy your requirements? We can figure the exact formulas later.

Edited by Álvaro, 31 July 2014 - 02:03 PM.


#3 olkeyn   Members   -  Reputation: 106

Like
0Likes
Like

Posted 01 August 2014 - 01:27 AM

Yes, you understood correctly.

I do not know how exactly realize what I want. I would assay all variants.



#4 Álvaro   Crossbones+   -  Reputation: 13326

Like
2Likes
Like

Posted 01 August 2014 - 07:04 AM

This should be close to what you need. It is a bit math heavy, so don't feel too bad if you don't understand it all: Just ask me about what parts you don't understand and I'll try to explain them.

#include <iostream>
#include <cmath>
#include <boost/math/quaternion.hpp>
 
typedef boost::math::quaternion<double> Quaternion;
 
struct Vector {
  double x, y, z;
  
  Vector(double x, double y, double z) : x(x), y(y), z(z) {
  }
};
 
std::ostream &operator<<(std::ostream &os, Vector v) {
  return os << '(' << v.x << ',' << v.y << ',' << v.z << ')';
}
 
Vector operator*(double s, Vector v) {
  return Vector(s * v.x, s * v.y, s * v.z);
}
 
Vector cross_product(Vector v1, Vector v2) {
  return Vector(v1.y * v2.z - v1.z * v2.y,
                v1.z * v2.x - v1.x * v2.z,
                v1.x * v2.y - v1.y * v2.x);
}
 
double dot_product(Vector v1, Vector v2) {
  return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
 
double squared_length(Vector v) {
  return dot_product(v, v);
}
 
double length(Vector v) {
  return std::sqrt(squared_length(v));
}
 
Quaternion most_natural_rotation(Vector v1, Vector v2) {
  Vector v = cross_product(v1, v2);
  Quaternion q(std::sqrt(squared_length(v1) * squared_length(v2)) + dot_product(v1, v2),
               v.x, v.y, v.z);
  return q / abs(q);
}
 
Vector apply_rotation(Quaternion q, Vector v) {
  Quaternion result = q * Quaternion(0.0, v.x, v.y, v.z) * conj(q);
  return Vector(result.R_component_2(), result.R_component_3(), result.R_component_4());
}
 
Vector log_of_unit_quaternion(Quaternion q) {
  double angle = std::acos(q.real());
  Vector axis(q.R_component_2(), q.R_component_3(), q.R_component_4());
  return (angle / length(axis)) * axis;
}
 
Vector compute_torque(Quaternion current_attitude,
                      Vector model_needle,
                      Vector target,
                      double spring_force) {
  Vector needle = apply_rotation(current_attitude, model_needle);
  Quaternion rotation = most_natural_rotation(needle, target);
  return spring_force * log_of_unit_quaternion(rotation);
}


Edited by Álvaro, 01 August 2014 - 07:05 AM.


#5 olkeyn   Members   -  Reputation: 106

Like
0Likes
Like

Posted 01 August 2014 - 11:58 AM

Thank you.
It seems I understand what that code doing. But as I try to do the same as your example and use it in my application, the body instead of aligning begins random rotation. Can you check? May be I doing something wrong, or it not exactly that needed.



#6 Waterlimon   Crossbones+   -  Reputation: 2565

Like
0Likes
Like

Posted 01 August 2014 - 12:07 PM

You probably also want to add a dampening term to the compute_torque function if im not wrong (-angular_velocity*dampening_force  appended to the return statement I think, of course add the variables as params), otherwise it will easily keep oscillating forever.

 

edit: or not if you actually want conservation of energy in your model...


Edited by Waterlimon, 01 August 2014 - 12:09 PM.

o3o


#7 Álvaro   Crossbones+   -  Reputation: 13326

Like
0Likes
Like

Posted 01 August 2014 - 12:53 PM

You probably do want dampening, but I think of dampening as something separate from the torque computation. In the analogous situation of simulating a pendulum, I wouldn't make my gravity term contain a dampening force. The code I wrote plays the role of computing the acceleration due to gravity. You also need friction.

#8 olkeyn   Members   -  Reputation: 106

Like
0Likes
Like

Posted 01 August 2014 - 03:55 PM

Dampening is next step. It would be more easier. Now it would be good if spring make harmonic vibration from start to target, opposite side and back, without unexpected rotation and flying into deep space.



#9 Álvaro   Crossbones+   -  Reputation: 13326

Like
1Likes
Like

Posted 02 August 2014 - 05:06 AM

Dampening is next step. It would be more easier. Now it would be good if spring make harmonic vibration from start to target, opposite side and back, without unexpected rotation and flying into deep space.


Is that not what you are getting?

I added a `main' and a couple of extra operators for vectors to show that integrating this torque works. You can set `angular_dampening' to 0.0 if you want to see it oscillate forever. For simplicity, I am assuming the moment of inertia is the identity matrix.

#include <iostream>
#include <cmath>
#include <boost/math/quaternion.hpp>

typedef boost::math::quaternion<double> Quaternion;

struct Vector {
  double x, y, z;
  
  Vector(double x, double y, double z) : x(x), y(y), z(z) {
  }
};

std::ostream &operator<<(std::ostream &os, Vector v) {
  return os << '(' << v.x << ',' << v.y << ',' << v.z << ')';
}

Vector operator*(double s, Vector v) {
  return Vector(s * v.x, s * v.y, s * v.z);
}

Vector operator-(Vector v1, Vector v2) {
  return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}

Vector &operator+=(Vector &v1, Vector v2) {
  v1.x += v2.x;
  v1.y += v2.y;
  v1.z += v2.z;
  
  return v1;
}

Vector cross_product(Vector v1, Vector v2) {
  return Vector(v1.y * v2.z - v1.z * v2.y,
		v1.z * v2.x - v1.x * v2.z,
		v1.x * v2.y - v1.y * v2.x);
}

double dot_product(Vector v1, Vector v2) {
  return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

double squared_length(Vector v) {
  return dot_product(v, v);
}

double length(Vector v) {
  return std::sqrt(squared_length(v));
}

Quaternion most_natural_rotation(Vector v1, Vector v2) {
  Vector v = cross_product(v1, v2);
  Quaternion q(std::sqrt(squared_length(v1) * squared_length(v2)) + dot_product(v1, v2),
	       v.x, v.y, v.z);
  return q / abs(q);
}

Vector apply_rotation(Quaternion q, Vector v) {
  Quaternion result = q * Quaternion(0.0, v.x, v.y, v.z) * conj(q);
  return Vector(result.R_component_2(), result.R_component_3(), result.R_component_4());
}

Vector log_of_unit_quaternion(Quaternion q) {
  double angle = std::acos(q.real());
  Vector axis(q.R_component_2(), q.R_component_3(), q.R_component_4());
  return (angle / length(axis)) * axis;
}

Vector compute_torque(Quaternion current_attitude,
		      Vector model_needle,
		      Vector target,
		      double spring_force) {
  Vector needle = apply_rotation(current_attitude, model_needle);
  Quaternion rotation = most_natural_rotation(needle, target);
  return spring_force * log_of_unit_quaternion(rotation);
}

int main() {
  Vector model_needle(1.0, 0.0, 0.0);
  Quaternion attitude(1.0, 0.0, 0.0, 0.0);
  Vector angular_velocity(0.0, 0.0, 0.0);
  
  double const angular_dampening = 1.0;
  double const dt = 0.1;
  
  for (int i=0; i<1000; ++i) {
    std::cout << apply_rotation(attitude, model_needle) << ' ' << attitude << ' ' << angular_velocity << '\n';
    Vector torque = compute_torque(attitude, model_needle, Vector(0.0, 1.0, 0.0), 1.0);
    Vector angular_acceleration = torque - angular_dampening * angular_velocity;
    angular_velocity += dt * angular_acceleration;
    attitude *= exp(dt * Quaternion(0.0, angular_velocity.x, angular_velocity.y, angular_velocity.z));
  }
}






Old topic!
Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.



PARTNERS