How to apply an orientation-dependent pure torque to Rigid body?

[jay]

Hi,
I hope everything is going well with you!
I’m currently working on simulating a permanent magnet in a uniform magnetic field.
I would like to create a custom forcefield that can apply pure torque to a permanent magnet generated by the external uniform magnetic field.

The torque generated by the magnetic field is

tau = mu X B,

where tau is torque vector, mu is magnetic moment vector and B is magnetic flux density vector. The operator “X” is the cross product, and all three vectors are 3-dimensional. The “mu” vector is fixed to the orientation of a rigid body, so mu rotates as the rigid body rotates. In other words, mu follows the quaternion of the rigid body.

If the torque was a constant vector, I could just use the ConstantForceField. However, since the torque is dependent upon the orientation of the permanent magnetic, the torque is not a constant vector. I was trying to create a custom force field for this magnetic torque, but I couldn’t find proper documentation about the rotation of 3d rigid body. It was quite painful for me to make a force field without knowing how SOFA handles the rotation, so I was wondering if you could give me some hints about it.

Questions:

I think the source code of TorsionForceField can be used as a reference. The addForce of the TorsionForceField is

template<>
void TorsionForceField<Rigid3Types>::addForce(const core::MechanicalParams *, DataVecDeriv &f, const DataVecCoord &x, const DataVecDeriv &/*v*/)
{
	const VecId& indices = m_indices.getValue();
	const VecCoord& q = x.getValue();
	const Real& tau = m_torque.getValue();
	const Pos& o = m_origin.getValue();
	VecDeriv& fq = *f.beginEdit();

	const auto nNodes = indices.size();

	for(Size n = 0 ; n < nNodes ; ++n)
	{
		PointId id = indices[n];
		const Pos t = tau*m_u;
		fq[id].getVCenter() += t.cross(q[id].getCenter() - (o + (q[id].getCenter() * m_u)*m_u) );
		fq[id].getVOrientation() += t;
	}

	f.endEdit();
}

1. what is the method getVCenter()? what does it return? what is the datatype and dof of the return?

2. what is the method getVOrientation()? what does it return? what is the datatype and dof of the return?

3. It seems to me that const Pos t in the above code is the torque. If I want to change the x value of torque vector, how can I do it? For example, if I want to change the torque vector [tx, ty, tz] to [47, ty, tz], how can I do it?

4. How can I get the quaternion of an element of DataVecCoord &x?
For example, how can I get the quaternion of x[0]?
Can I just use the following code?
Pos& quaternion = x[0].getVOrientation()

4-2. A quaternion is composed of 4 values. How can I get them separately?
For example, how can I store the direction vector part and angle part of a quaternion to each variable?

5. How can I apply pure torque in the source code? For example, if I want to apply pure torque and its vector is [10, 5, 1], how can I apply this torque?

Lastly, could you explain how to deal with addDForce of the torque?
I found this code from the TorsionForceField.

template<>
void TorsionForceField<Rigid3Types>::addDForce(const core::MechanicalParams *mparams, DataVecDeriv &df, const DataVecDeriv &dx)
{
	const VecId& indices = m_indices.getValue();
	const VecDeriv& dq = dx.getValue();
	const Real& tau = m_torque.getValue();
	VecDeriv& dfq = *df.beginEdit();

	const auto nNodes = indices.size();
	const Real& kfact = mparams->kFactor();

	Mat3 D;
	D(0,0) = 1 - m_u(0)*m_u(0) ;	D(0,1) = -m_u(1)*m_u(0) ;		D(0,2) = -m_u(2)*m_u(0);
	D(1,0) = -m_u(0)*m_u(1) ;		D(1,1) = 1 - m_u(1)*m_u(1) ;	D(1,2) = -m_u(2)*m_u(1);
	D(2,0) = -m_u(0)*m_u(2) ;		D(2,1) = -m_u(1)*m_u(2) ;		D(2,2) = 1 - m_u(3)*m_u(3);
	D *= (tau * kfact);

	for(Size n = 0 ; n < nNodes ; ++n)
	{
		PointId id = indices[n];
		dfq[id].getVCenter() += D * dq[id].getVCenter();
	}
}

6. I know I have to add some kind of jacobian in this addDForce, but I have no idea what should be the jacobian of the quaternion. I think the matrix D is the jacobian matrix, but I can’t understand what it means. Could you tell me what should be the jacobian matrix when applying pure torque? In my environment, the torque is orientation-dependent, so I want to know the general theory behind it so that I can create my own addDForce.

Thank you so much for taking your time.
Best,
Jay

[Hugo]

Hey @jay-h

Very interesting topic, thanks for your message.
I am sorry to read that your investigation was painful. I am now here to make sure the pain vanish!

1. With Rigid3d (rigid) objects, the degrees of freedom (dofs) in SOFA are :
– position with 7 dofs: [x, y, z] + quaternion (3 positions, 4 orientation)
– velocities with 6 dofs : [vx, vy, vz], [tx, ty, tz] (3 velocities, 3 torques)

getVCenter() is a method which returns the velocity part [vx, vy, vz] of state vector. In the addForce function, the vector “fq” is the resulting force vector (accumulated in the b vector if our system looks like Ax=b) and “fq.getVCenter()” correponds to [fx, fy, fz].

2. Therefore, on the “fq” force vector, the function getVOrientation() returns the torques [tx, ty, tz].

3. Pos corresponds to the torque [tx, ty, tz] –> typedef type::Vec<3,real> Pos;, you should therefore be able to access it by writing :

const Pos t = tau*m_u;
t[0] = 47;

4. To get the quaternion of x[0], you can write x[0].getOrientation() if x is the position state vector.
If x corresponds to the velocity state or a force vector it should be written: x[0].getVOrientation() (you would then recover the torque as above)

4-2. To get the quaternion values separately, you can do q[0], q[1], q[2], q[3].

5. To apply pure torque, let’s take the example of the TorsionFF again, all you need is to ONLY define: fq[id].getVOrientation() += ... with fq[id].getVCenter() = 0;. To do what you propose:

fq[id].getVOrientation()[0] = 10;
fq[id].getVOrientation()[1] = 5;
fq[id].getVOrientation()[2] = 1;

in the source code? For example, if I want to apply pure torque and its vector is [10, 5, 1], how can I apply this torque?

6 – How to deal with addDForce: the addDForce function is indeed the derivative of the force function with regards to the degrees of freedom.
Looking at the doc of the EulerImplicit, addDForce computes

In the TorsionForceField, the matrix D is indeed the jacobian matrix.
To implement your addDForce the question is : what is the expression of the force with regards on the degrees of freedom. Since your force (torque) depends on the orientation you should implement addDForce. What is this relationship ?

Best wishes,

Best wishes,

Hugo

PS: I must say that modeling magnets in SOFA is a topic of interest. It would be amazing to have you presenting part of your work at the SOFA Week 2021. I can already tell you that there would be an audience for this!

[jay]

Hi Hugo,

Thank you so much for your reply.
Now I can understand how SOFA treats the rotation of a rigid body.

Your reply clearly answered most of the questions, but can I ask just one more question?
Regarding question #6, I think the jacobian matrix (df/dx) of a rigid body should be a 6 by 7 matrix since f has 6 components and x has 7 components.

My questions are :

Q1.
In this structure, I guess the following approach is the right way of adding the addDforce in SOFA. If you don’t mind, could you check whether it is correct?

– Let’s assume that we have a jacobian matrix, M, which is 6 by 7 matrix,

  M = [df1/dx1  df1/dx2  df1/dx3  df1/dq1  df1/dq2  df1/dq3  df1/dq4
       df2/dx1  df2/dx2  df2/dx3  df2/dq1  df2/dq2  df2/dq3  df2/dq4
       df3/dx1  df3/dx2  df3/dx3  df3/dq1  df3/dq2  df3/dq3  df3/dq4
       dt1/dx1  dt1/dx2  dt1/dx3  dt1/dq1  dt1/dq2  dt1/dq3  dt1/dq4
       dt2/dx1  dt2/dx2  dt2/dx3  dt2/dq1  dt2/dq2  dt2/dq3  dt2/dq4
       dt3/dx1  dt3/dx2  dt3/dx3  dt3/dq1  dt3/dq2  dt3/dq3  dt3/dq4]

where f is force(translation), t is torque, x is position(translation), q is quaternion.

– Now split M into four parts – Mfx, Mfq, Mtx, Mtq.

  # split M into four parts
  M = [Mfx                       | Mfq
       __________________________|_____
       Mtx                       | Mtq],

  # Where
Mfx = [df1/dx1  df1/dx2  df1/dx3
       df2/dx1  df2/dx2  df2/dx3
       df3/dx1  df3/dx2  df3/dx3]
Mfq = [df1/dq1  df1/dq2  df1/dq3  df1/dq4
       df2/dq1  df2/dq2  df2/dq3  df2/dq4
       df3/dq1  df3/dq2  df3/dq3  df3/dq4]
Mtx = [dt1/dx1  dt1/dx2  dt1/dx3
       dt2/dx1  dt2/dx2  dt2/dx3
       dt3/dx1  dt3/dx2  dt3/dx3]
Mtq = [dt1/dq1  dt1/dq2  dt1/dq3  dt1/dq4
       dt2/dq1  dt2/dq2  dt2/dq3  dt2/dq4
       dt3/dq1  dt3/dq2  dt3/dq3  dt3/dq4]

– Then, the derivative of the force is calculated by the following equations

template<>
void CustomForceField<Rigid3Types>::addDForce(const core::MechanicalParams *mparams, DataVecDeriv &df, const DataVecDeriv &dx)
{
    const VecDeriv& dq = dx.getValue();
    VecDeriv& dfq = *df.beginEdit();

    ## Mfx, Mfq, Mtx, Mtq are calculated here
    ## Mfx = ~~~
    ## Mfq = ~~~
    ## Mtx = ~~~
    ## Mtq = ~~~

    for(Size n = 0 ; n < nNodes ; ++n)
    {
        PointId id = indices[n];
        dfq[id].getVCenter() += Mfx * dq[id].getCenter();
        dfq[id].getVCenter() += Mfq * dq[id].getOrientation();
        dfq[id].getVOrientation() += Mtx * dq[id].getCenter();
        dfq[id].getVOrientation() += Mtq * dq[id].getOrientation();
    }
}

Is this correct?

Q2.
A 3 by 3 Matrix can be created by

  Mat3 D;
  D(0,0) = 1 ; D(0,1) = 0 ; D(0,2) = 0;
  D(0,0) = 0 ; D(0,1) = 1 ; D(0,2) = 0;
  D(0,0) = 0 ; D(0,1) = 0 ; D(0,2) = 1;

However, we need 3 by 4 matrix for Mfq, Mtq.
How do you create 3 by 4 matrix? What is the name of the datatype?

Many thanks,
Jay

[jay]

Hi @Hugo,
I just found out that the dx for addDforce (const DataVecDeriv &dx) is not a 4d vector (quaternion), but a 3d vector.
Could you tell me how to compute the jacobian of a rigid body rotation which also depends on the theta term of the quaternion(denoted as q4 in the previous question)?

[Hugo]

Hey @jay-h

Indeed, dx in the addDforce function corresponds to a delta of velocity (the unknown vector x of your linear system Ax=b). Rigid3d velocities are indeed a 3d vector.

How would you express your rigid body rotation against Euler angles?

Best,

Hugo

[Author]

Posts