/// Set body-relative coord. and update auxiliary variables /// Also, current position becomes the 'resting position' coordinates /// for the current time. public void Impose_Rel_Coord(ChCoordsys m_coord) { ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0); // set the actual coordinates FrameMoving.SetCoord(m_coord); // set the resting position coordinates rest_coord.pos.x = m_coord.pos.x - motion_X.Get_y(ChTime); rest_coord.pos.y = m_coord.pos.y - motion_Y.Get_y(ChTime); rest_coord.pos.z = m_coord.pos.z - motion_Z.Get_y(ChTime); qtemp = ChQuaternion.Q_from_AngAxis2(-(motion_ang.Get_y(ChTime)), motion_axis); rest_coord.rot = ChQuaternion.Qcross(m_coord.rot, qtemp); // ***%%% check // set also the absolute positions, and other. UpdateState(); }
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++; } } }
// // UPDATING // /// Updates the time.dependant variables (ex: ChFunction objects /// which impose the body-relative motion, etc.) public void UpdateTime(double mytime) { ChCoordsys csys = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChCoordsys csys_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChCoordsys csys_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0); double ang, ang_dt, ang_dtdt; ChTime = mytime; // if a imposed motion (keyframed movement) affects the marker position (example,from R3D animation system), // compute the speed and acceleration values by BDF (example,see the UpdatedExternalTime() function, later) // so the updating via motion laws can be skipped! if (motion_type == eChMarkerMotion.M_MOTION_KEYFRAMED) { return; } // skip relative-position-functions evaluation also if // someone is already handling this from outside.. if (motion_type == eChMarkerMotion.M_MOTION_EXTERNAL) { return; } // positions: // update positions: rel_pos csys.pos.x = motion_X.Get_y(mytime); csys.pos.y = motion_Y.Get_y(mytime); csys.pos.z = motion_Z.Get_y(mytime); if (motion_X.Get_Type() != ChFunction.FunctionType.FUNCT_MOCAP) { csys.pos += rest_coord.pos; } // update speeds: rel_pos_dt csys_dt.pos.x = motion_X.Get_y_dx(mytime); csys_dt.pos.y = motion_Y.Get_y_dx(mytime); csys_dt.pos.z = motion_Z.Get_y_dx(mytime); // update accelerations csys_dtdt.pos.x = motion_X.Get_y_dxdx(mytime); csys_dtdt.pos.y = motion_Y.Get_y_dxdx(mytime); csys_dtdt.pos.z = motion_Z.Get_y_dxdx(mytime); // rotations: ang = motion_ang.Get_y(mytime); ang_dt = motion_ang.Get_y_dx(mytime); ang_dtdt = motion_ang.Get_y_dxdx(mytime); if ((ang != 0) || (ang_dt != 0) || (ang_dtdt != 0)) { // update q ChVector motion_axis_versor = ChVector.Vnorm(motion_axis); qtemp = ChQuaternion.Q_from_AngAxis2(ang, motion_axis_versor); csys.rot = ChQuaternion.Qcross(qtemp, rest_coord.rot); // update q_dt csys_dt.rot = ChQuaternion.Qdt_from_AngAxis(csys.rot, ang_dt, motion_axis_versor); // update q_dtdt csys_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(ang_dtdt, motion_axis_versor, csys.rot, csys_dt.rot); } else { csys.rot = FrameMoving.coord.rot; // rel_pos.rot; csys_dt.rot = ChQuaternion.QNULL; csys_dtdt.rot = ChQuaternion.QNULL; } // Set the position, speed and acceleration in relative space, // automatically getting also the absolute values, if (!(csys == this.FrameMoving.coord)) { FrameMoving.SetCoord(csys); } if (!(csys_dt == this.FrameMoving.coord_dt) || !(csys_dt.rot == new ChQuaternion(0, 0, 0, 0))) { FrameMoving.SetCoord_dt(csys_dt); } if (!(csys_dtdt == this.FrameMoving.coord_dtdt) || !(csys_dtdt.rot == new ChQuaternion(0, 0, 0, 0))) { FrameMoving.SetCoord_dtdt(csys_dtdt); } }
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 = this.frame1.BitShiftRight(this.Body1); ChFrame <double> aframe2 = this.frame2.BitShiftRight(this.Body2); ChFrame <double> aframe12 = new ChFrame <double>();// ChFrame<double>.FNULL; aframe2.TransformParentToLocal(aframe1, aframe12); ChFrame <double> aframe2rotating = new ChFrame <double>();// ChFrame<double>.FNULL; double aux_rotation; aux_rotation = m_func.Get_y(mytime) + rot_offset; aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z)); ChFrame <double> aframe12rotating = new ChFrame <double>(); // aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating); ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0), Jw2 = new ChMatrix33 <double>(0); ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0), mtempQ = new ChMatrix33 <double>(0); 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(); // 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.nm.matrix[0, 0] = 0.5 * aframe12rotating.GetRot().e0; * mtempM.nm.matrix[1, 1] = 0.5 * aframe12rotating.GetRot().e0; * mtempM.nm.matrix[2, 2] = 0.5 * aframe12rotating.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) { 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++; } } }
/// Updates motion laws, etc. for the impose rotation / impose speed modes public override void UpdateTime(double mytime) { // First, inherit to parent class base.UpdateTime(mytime); if (!IsActive()) { return; } // DEFAULTS compute rotation vars... // by default for torque control.. motion_axis = ChVector.VECT_Z; // motion axis is always the marker2 Z axis (in m2 relative coords) mot_rot = relAngle; mot_rot_dt = ChVector.Vdot(relWvel, motion_axis); mot_rot_dtdt = ChVector.Vdot(relWacc, motion_axis); mot_rerot = mot_rot / mot_tau; mot_rerot_dt = mot_rot_dt / mot_tau; mot_rerot_dtdt = mot_rot_dtdt / mot_tau; // nothing more to do here for torque control if (eng_mode == eCh_eng_mode.ENG_MODE_TORQUE) { return; } // If LEARN MODE, just record motion if (learn) { deltaC.pos = ChVector.VNULL; deltaC_dt.pos = ChVector.VNULL; deltaC_dtdt.pos = ChVector.VNULL; if (!(limit_Rx.Get_active() || limit_Ry.Get_active() || limit_Rz.Get_active())) { deltaC.rot = ChQuaternion.QUNIT; deltaC_dt.rot = ChQuaternion.QNULL; deltaC_dtdt.rot = ChQuaternion.QNULL; } if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION) { if (rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) { rot_funct = new ChFunction_Recorder(); } // record point double rec_rot = relAngle; // ***TO DO*** compute also rotations with cardano mode? if (impose_reducer) { rec_rot = rec_rot / mot_tau; } ChFunction_Recorder rec = (ChFunction_Recorder)rot_funct; rec.AddPoint(mytime, rec_rot, 1); // x=t } if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED) { if (spe_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) { spe_funct = new ChFunction_Recorder(); } // record point double rec_spe = ChVector.Vlength(relWvel); // ***TO DO*** compute also with cardano mode? if (impose_reducer) { rec_spe = rec_spe / mot_tau; } ChFunction_Recorder rec = (ChFunction_Recorder)spe_funct; rec.AddPoint(mytime, rec_spe, 1); // x=t } } if (learn) { return; // no need to go on further...--.>>> } // Impose relative positions/speeds deltaC.pos = ChVector.VNULL; deltaC_dt.pos = ChVector.VNULL; deltaC_dtdt.pos = ChVector.VNULL; if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION) { if (impose_reducer) { mot_rerot = rot_funct.Get_y(ChTime); mot_rerot_dt = rot_funct.Get_y_dx(ChTime); mot_rerot_dtdt = rot_funct.Get_y_dxdx(ChTime); mot_rot = mot_rerot * mot_tau; mot_rot_dt = mot_rerot_dt * mot_tau; mot_rot_dtdt = mot_rerot_dtdt * mot_tau; } else { mot_rot = rot_funct.Get_y(ChTime); mot_rot_dt = rot_funct.Get_y_dx(ChTime); mot_rot_dtdt = rot_funct.Get_y_dxdx(ChTime); mot_rerot = mot_rot / mot_tau; mot_rerot_dt = mot_rot_dt / mot_tau; mot_rerot_dtdt = mot_rot_dtdt / mot_tau; } deltaC.rot = ChQuaternion.Q_from_AngAxis2(mot_rot, motion_axis); deltaC_dt.rot = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis); deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot); } if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED) { if (impose_reducer) { mot_rerot_dt = spe_funct.Get_y(ChTime); mot_rerot_dtdt = spe_funct.Get_y_dx(ChTime); mot_rot_dt = mot_rerot_dt * mot_tau; mot_rot_dtdt = mot_rerot_dtdt * mot_tau; } else { mot_rot_dt = spe_funct.Get_y(ChTime); mot_rot_dtdt = spe_funct.Get_y_dx(ChTime); mot_rerot_dt = mot_rot_dt / mot_tau; mot_rerot_dtdt = mot_rot_dtdt / mot_tau; } deltaC.rot = ChQuaternion.Qnorm(GetRelM().rot); // just keep current position, -assume always good after integration-. ChMatrix33 <double> relA = new ChMatrix33 <double>(0); relA.Set_A_quaternion(GetRelM().rot); // ..but adjust to keep Z axis aligned to shaft, anyway! ChVector displaced_z_axis = relA.Get_A_Zaxis(); ChVector adjustment = ChVector.Vcross(displaced_z_axis, ChVector.VECT_Z); deltaC.rot = ChQuaternion.Q_from_AngAxis2(ChVector.Vlength(adjustment), ChVector.Vnorm(adjustment)) % deltaC.rot; deltaC_dt.rot = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis); deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot); } }