Beispiel #1
0
        /// Get the link coordinate system, expressed relative to Body2 (spherical side).
        /// This represents the 'main' reference of the link: reaction forces
        /// and reaction torques are reported in this coordinate system.
        public override ChCoordsys GetLinkRelativeCoords()
        {
            ChVector            pos1 = Body2.TransformPointParentToLocal(Body1.TransformPointLocalToParent(m_pos1));
            ChMatrix33 <double> A    = new ChMatrix33 <double>(0);

            ChVector u = (m_pos2 - pos1).GetNormalized();
            ChVector w = Body2.TransformDirectionParentToLocal(Body1.TransformDirectionLocalToParent(m_dir1));
            ChVector v = ChVector.Vcross(w, u);

            A.Set_A_axis(u, v, w);

            return(new ChCoordsys(pos1, A.Get_A_quaternion()));
        }
Beispiel #2
0
        //
        // UPDATING FUNCTIONS
        //

        /// Perform the update of this joint at the specified time: compute jacobians,
        /// raint violations, etc. and cache in internal structures
        public override void update(double time, bool update_assets = true)
        {
            // Inherit time changes of parent class (ChLink)
            base.update(time, update_assets);

            // Express the body locations and direction in absolute frame
            ChVector pos1_abs = Body1.TransformPointLocalToParent(m_pos1);
            ChVector pos2_abs = Body2.TransformPointLocalToParent(m_pos2);
            ChVector dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1);
            ChVector d12_abs  = pos2_abs - pos1_abs;

            // Update current distance and dot product
            m_cur_dist = d12_abs.Length();
            m_cur_dot  = ChVector.Vdot(d12_abs, dir1_abs);

            // Calculate a unit vector in the direction d12, expressed in absolute frame
            // Then express it in the two body frames
            ChVector u12_abs  = d12_abs / m_cur_dist;
            ChVector u12_loc1 = Body1.TransformDirectionParentToLocal(u12_abs);
            ChVector u12_loc2 = Body2.TransformDirectionParentToLocal(u12_abs);

            // Express the direction vector in the frame of body 2
            ChVector dir1_loc2 = Body2.TransformDirectionParentToLocal(dir1_abs);

            // Cache violation of the distance constraint
            m_C.matrix.SetElement(0, 0, m_cur_dist - m_dist);

            // Compute Jacobian of the distance constraint
            //    ||pos2_abs - pos1_abs|| - dist = 0
            {
                ChVector Phi_r1  = -u12_abs;
                ChVector Phi_pi1 = ChVector.Vcross(u12_loc1, m_pos1);

                m_cnstr_dist.Get_Cq_a().ElementN(0) = Phi_r1.x;
                m_cnstr_dist.Get_Cq_a().ElementN(1) = Phi_r1.y;
                m_cnstr_dist.Get_Cq_a().ElementN(2) = Phi_r1.z;

                m_cnstr_dist.Get_Cq_a().ElementN(3) = Phi_pi1.x;
                m_cnstr_dist.Get_Cq_a().ElementN(4) = Phi_pi1.y;
                m_cnstr_dist.Get_Cq_a().ElementN(5) = Phi_pi1.z;

                ChVector Phi_r2  = u12_abs;
                ChVector Phi_pi2 = -ChVector.Vcross(u12_loc2, m_pos2);

                m_cnstr_dist.Get_Cq_b().ElementN(0) = Phi_r2.x;
                m_cnstr_dist.Get_Cq_b().ElementN(1) = Phi_r2.y;
                m_cnstr_dist.Get_Cq_b().ElementN(2) = Phi_r2.z;

                m_cnstr_dist.Get_Cq_b().ElementN(3) = Phi_pi2.x;
                m_cnstr_dist.Get_Cq_b().ElementN(4) = Phi_pi2.y;
                m_cnstr_dist.Get_Cq_b().ElementN(5) = Phi_pi2.z;
            }

            // Cache violation of the dot constraint
            m_C.matrix.SetElement(1, 0, m_cur_dot);

            // Compute Jacobian of the dot constraint
            //    dot(dir1_abs, pos2_abs - pos1_abs) = 0
            {
                ChVector Phi_r1  = -dir1_abs;
                ChVector Phi_pi1 = ChVector.Vcross(m_dir1, m_pos1) - ChVector.Vcross(u12_loc1, m_pos1);

                m_cnstr_dot.Get_Cq_a().ElementN(0) = Phi_r1.x;
                m_cnstr_dot.Get_Cq_a().ElementN(1) = Phi_r1.y;
                m_cnstr_dot.Get_Cq_a().ElementN(2) = Phi_r1.z;

                m_cnstr_dot.Get_Cq_a().ElementN(3) = Phi_pi1.x;
                m_cnstr_dot.Get_Cq_a().ElementN(4) = Phi_pi1.y;
                m_cnstr_dot.Get_Cq_a().ElementN(5) = Phi_pi1.z;

                ChVector Phi_r2  = dir1_abs;
                ChVector Phi_pi2 = -ChVector.Vcross(dir1_loc2, m_pos2);

                m_cnstr_dot.Get_Cq_b().ElementN(0) = Phi_r2.x;
                m_cnstr_dot.Get_Cq_b().ElementN(1) = Phi_r2.y;
                m_cnstr_dot.Get_Cq_b().ElementN(2) = Phi_r2.z;

                m_cnstr_dot.Get_Cq_b().ElementN(3) = Phi_pi2.x;
                m_cnstr_dot.Get_Cq_b().ElementN(4) = Phi_pi2.y;
                m_cnstr_dot.Get_Cq_b().ElementN(5) = Phi_pi2.z;
            }
        }
Beispiel #3
0
 /// Get the direction of the revolute joint, expressed in absolute coordinate system.
 public ChVector GetDir1Abs()
 {
     return(Body1.TransformDirectionLocalToParent(m_dir1));
 }