Home › Forum › SOFA › Programming with SOFA › How to apply an orientation-dependent pure torque to Rigid body?
Tagged: Linux_ubuntu, SOFA_2106
- This topic has 4 replies, 2 voices, and was last updated 3 years ago by Hugo.
-
AuthorPosts
-
11 August 2021 at 18:16 #20154jayBlocked
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,
Jay23 August 2021 at 19:14 #20216HugoKeymasterHey @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() += ...
withfq[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 computesIn 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!
24 August 2021 at 23:56 #20251jayBlockedHi 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 byMat3 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,
Jay3 September 2021 at 17:34 #20305jayBlockedHi @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)?7 October 2021 at 10:45 #20544 -
AuthorPosts
- You must be logged in to reply to this topic.