// // SOLVER INTERFACE (OLD) // public override void ConstraintsFbLoadForces(double factor = 1) { // compute instant torque double mT = m_func.Get_y(this.GetChTime()); ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1)); ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2)); ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT)); Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3, 0); Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0); }
/// Initialize this joint by specifying the two bodies to be connected and the /// joint frames on each body. If local = true, it is assumed that these quantities /// are specified in the local body frames. Otherwise, it is assumed that they are /// specified in the absolute frame. public void Initialize(ChBodyFrame body1, //< first body frame ChBodyFrame body2, //< second body frame bool local, //< true if data given in body local frames ChFrame <double> frame1, //< joint frame on body 1 ChFrame <double> frame2 //< joint frame on body 2 ) { Body1 = body1; Body2 = body2; m_cnstr_x.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_y.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_z.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables()); ChFrame <double> frame1_abs; ChFrame <double> frame2_abs; if (local) { m_frame1 = frame1; m_frame2 = frame2; frame1_abs = ChFrame <double> .BitShiftRight(frame1, Body1); frame2_abs = ChFrame <double> .BitShiftRight(frame2, Body2); } else { ((ChFrame <double>)Body1).TransformParentToLocal(frame1, m_frame1); ((ChFrame <double>)Body2).TransformParentToLocal(frame2, m_frame2); frame1_abs = frame1; frame2_abs = frame2; } m_u1_tilde.Set_X_matrix(m_frame1.GetA().Get_A_Xaxis()); m_v2_tilde.Set_X_matrix(m_frame2.GetA().Get_A_Yaxis()); m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x); m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y); m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z); m_C.matrix.SetElement(3, 0, ChVector.Vdot(frame1_abs.GetA().Get_A_Xaxis(), frame2_abs.GetA().Get_A_Yaxis())); }
// // STATE FUNCTIONS // public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c) { // compute instant torque double mT = m_func.Get_y(this.GetChTime()); ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1)); ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2)); ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT)); if (Body2.Variables().IsActive()) { R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c, Body2.Variables().GetOffset() + 3, 0); } if (Body1.Variables().IsActive()) { R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c, Body1.Variables().GetOffset() + 3, 0); } }
public override void update(double mytime, bool update_assets) { // Inherit parent class: base.update(mytime, update_assets); // Override the rotational jacobian [Cq] and the rotational residual C, // by assuming an additional hidden frame that rotates about frame2: if (this.Body1 != null && this.Body2 != null) { ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1)); ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2)); ChFrame <double> aframe12 = new ChFrame <double>(); aframe2.TransformParentToLocal(aframe1, aframe12); ChFrame <double> aframe2rotating = new ChFrame <double>(); double aux_rotation; if (this.avoid_angle_drift) { aux_rotation = this.aux_dt + this.rot_offset; } else { // to have it aligned to current rot, to allow C=0. aux_rotation = aframe12.GetRot().Q_to_Rotv().z; } aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z)); // TODO this needs to be addressed, with it it causes rotation problems, seems to work fine without the TransformParentToLocal? ChFrame <double> aframe12rotating = new ChFrame <double>(); // aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating); ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(); ChMatrix33 <double> Jw2 = new ChMatrix33 <double>(); ChMatrix33 <double> mtempM = new ChMatrix33 <double>(); ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(); ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA(); Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix); Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().nm.matrix); Jw2.nm.matrix.MatrNeg(); // TODO this also needs to be addressed, with it it causes rotation problems/ // Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion. /* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5); * mtempM[0, 0] = 0.5 * aframe12rotating.GetRot().e0; * mtempM[1, 1] = 0.5 * aframe12rotating.GetRot().e0; * mtempM[2, 2] = 0.5 * aframe12rotating.GetRot().e0; * mtempQ.MatrTMultiply(mtempM, Jw1); * Jw1 = mtempQ; * mtempQ.MatrTMultiply(mtempM, Jw2); * Jw2 = mtempQ;*/ int nc = 0; if (c_x) { nc++; } if (c_y) { nc++; } if (c_z) { nc++; } if (c_rx) { this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e1; this.mask.Constr_N(nc).Get_Cq_a().FillElem(0); this.mask.Constr_N(nc).Get_Cq_b().FillElem(0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3); nc++; } if (c_ry) { this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e2; this.mask.Constr_N(nc).Get_Cq_a().FillElem(0); this.mask.Constr_N(nc).Get_Cq_b().FillElem(0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3); nc++; } if (c_rz) { this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e3; this.mask.Constr_N(nc).Get_Cq_a().FillElem(0); this.mask.Constr_N(nc).Get_Cq_b().FillElem(0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3); nc++; } } }
/// Get the joint frame on Body2, expressed in absolute coordinate system. public ChFrame <double> GetFrame2Abs() { return(ChFrame <double> .BitShiftRight(m_frame2, Body2)); }
/// Get the joint frame on Body1, expressed in absolute coordinate system. public ChFrame <double> GetFrame1Abs() { return(ChFrame <double> .BitShiftRight(m_frame1, Body1)); }
// // UPDATING FUNCTIONS // /// Override _all_ time, jacobian etc. updating. public override void update(double mytime, bool update_assets = true) { // Inherit time changes of parent class (ChLink), basically doing nothing :) base.update(mytime, update_assets); if (this.Body1 != null && this.Body2 != null) { this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables()); ChFrame <double> aframe = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1)); ChVector p1_abs = aframe.GetPos(); ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2)); ChVector p2_abs = aframe2.GetPos(); ChFrame <double> bframe = new ChFrame <double>(); (this.Body2).TransformParentToLocal(aframe, bframe); this.frame2.TransformParentToLocal(bframe, aframe); // Now 'aframe' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords. //***TODO*** check if it is faster to do aframe2.TransformParentToLocal(aframe, bframe); instead of two transforms above ChMatrix33 <double> Jx1 = new ChMatrix33 <double>(0); ChMatrix33 <double> Jx2 = new ChMatrix33 <double>(0); ChMatrix33 <double> Jr1 = new ChMatrix33 <double>(0); ChMatrix33 <double> Jr2 = new ChMatrix33 <double>(0); ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0); ChMatrix33 <double> Jw2 = new ChMatrix33 <double>(0); ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0); ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(0); ChMatrix33 <double> abs_plane = new ChMatrix33 <double>(0); abs_plane.nm.matrix.MatrMultiply(Body2.GetA().nm.matrix, frame2.GetA().nm.matrix); Jx1.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix); Jx2.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix); Jx2.nm.matrix.MatrNeg(); Jw1.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body1.GetA().nm.matrix); Jw2.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body2.GetA().nm.matrix); mtempM.Set_X_matrix(frame1.GetPos()); Jr1.nm.matrix.MatrMultiply(Jw1.nm.matrix, mtempM.nm.matrix); Jr1.nm.matrix.MatrNeg(); mtempM.Set_X_matrix(frame2.GetPos()); Jr2.nm.matrix.MatrMultiply(Jw2.nm.matrix, mtempM.nm.matrix); ChVector p2p1_base2 = (Body2.GetA()).MatrT_x_Vect(ChVector.Vsub(p1_abs, p2_abs)); mtempM.Set_X_matrix(p2p1_base2); mtempQ.nm.matrix.MatrTMultiply(frame2.GetA().nm.matrix, mtempM.nm.matrix); Jr2.nm.matrix.MatrInc(mtempQ.nm.matrix); Jw2.nm.matrix.MatrNeg(); // Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion. // For small misalignment this effect is almost insignificant cause [Fp(q_resid)]=[I], // but otherwise it is needed (if you want to use the stabilization term - if not, you can live without). mtempM.Set_X_matrix((aframe.GetRot().GetVector()) * 0.5); mtempM.nm.matrix[0, 0] = 0.5 * aframe.GetRot().e0; mtempM.nm.matrix[1, 1] = 0.5 * aframe.GetRot().e0; mtempM.nm.matrix[2, 2] = 0.5 * aframe.GetRot().e0; mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix); Jw1 = mtempQ; mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix); Jw2 = mtempQ; int nc = 0; if (c_x) { this.C.matrix.ElementN(nc) = aframe.GetPos().x; this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 0, 0, 1, 3, 0, 0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 0, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 0, 0, 1, 3, 0, 0); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 0, 0, 1, 3, 0, 3); nc++; } if (c_y) { this.C.matrix.ElementN(nc) = aframe.GetPos().y; this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 1, 0, 1, 3, 0, 0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 1, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 1, 0, 1, 3, 0, 0); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 1, 0, 1, 3, 0, 3); nc++; } if (c_z) { this.C.matrix.ElementN(nc) = aframe.GetPos().z; this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 2, 0, 1, 3, 0, 0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 2, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 2, 0, 1, 3, 0, 0); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 2, 0, 1, 3, 0, 3); nc++; } if (c_rx) { this.C.matrix.ElementN(nc) = aframe.GetRot().e1; this.mask.Constr_N(nc).Get_Cq_a().FillElem(0); this.mask.Constr_N(nc).Get_Cq_b().FillElem(0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3); nc++; } if (c_ry) { this.C.matrix.ElementN(nc) = aframe.GetRot().e2; this.mask.Constr_N(nc).Get_Cq_a().FillElem(0); this.mask.Constr_N(nc).Get_Cq_b().FillElem(0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3); nc++; } if (c_rz) { this.C.matrix.ElementN(nc) = aframe.GetRot().e3; this.mask.Constr_N(nc).Get_Cq_a().FillElem(0); this.mask.Constr_N(nc).Get_Cq_b().FillElem(0); this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3); this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3); nc++; } } }