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?

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.

Yes, you understood correctly.

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

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);
}

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.

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...

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.

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.

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));
  }
}

alvaro said:
log_of_unit_quaternion

I was wondering how did you choose this name for the function?

Why log? log as in logarithm doesn't make sense to me.

Or why log?

(Oh, I think you meant log as in logging information? Saving information?)

But, I don't understand this function.. It takes the rotation as argument, and scales the axis part of this rotation quaternion, right?

Why does this lead to values, we can use as a torque vector?