/// Initialize this joint by specifying the two bodies to be connected, a point /// and a direction on body1 defining the revolute joint, and a point on the /// second body defining the spherical joint. 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. The imposed /// distance between the two points can be either inferred from the provided /// configuration (auto_distance = true) or specified explicitly. public void Initialize(ChBodyFrame body1, //< first frame (revolute side) ChBodyFrame body2, //< second frame (spherical side) bool local, //< true if data given in body local frames ChVector pos1, //< point on first frame (center of revolute) ChVector dir1, //< direction of revolute on first frame ChVector pos2, //< point on second frame (center of spherical) bool auto_distance = true, //< true if imposed distance equal to |pos1 - po2| double distance = 0 //< imposed distance (used only if auto_distance = false) ) { Body1 = body1; Body2 = body2; m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables()); ChVector pos1_abs; ChVector pos2_abs; ChVector dir1_abs; if (local) { m_pos1 = pos1; m_pos2 = pos2; m_dir1 = ChVector.Vnorm(dir1); pos1_abs = Body1.TransformPointLocalToParent(m_pos1); pos2_abs = Body2.TransformPointLocalToParent(m_pos2); dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1); } else { pos1_abs = pos1; pos2_abs = pos2; dir1_abs = ChVector.Vnorm(dir1); m_pos1 = Body1.TransformPointParentToLocal(pos1_abs); m_pos2 = Body2.TransformPointParentToLocal(pos2_abs); m_dir1 = Body1.TransformDirectionParentToLocal(dir1_abs); } ChVector d12_abs = pos2_abs - pos1_abs; m_cur_dist = d12_abs.Length(); m_dist = auto_distance ? m_cur_dist : distance; m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs); }
/// 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())); }
/// Initialize this joint by specifying the two bodies to be connected, a /// coordinate system specified in the absolute frame, and the distance of /// the massless connector. The composite joint is ructed such that the /// direction of the revolute joint is aligned with the z axis of the specified /// coordinate system and the spherical joint is at the specified distance /// along the x axis. public void Initialize(ChBodyFrame body1, //< first frame (revolute side) ChBodyFrame body2, //< second frame (spherical side) ChCoordsys csys, //< joint coordinate system (in absolute frame) double distance //< imposed distance ) { Body1 = body1; Body2 = body2; m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables()); m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables()); ChVector x_Axis = csys.rot.GetXaxis(); ChVector z_axis = csys.rot.GetZaxis(); m_pos1 = Body1.TransformPointParentToLocal(csys.pos); m_dir1 = Body1.TransformDirectionParentToLocal(z_axis); m_pos2 = Body2.TransformPointParentToLocal(csys.pos + distance * x_Axis); m_dist = distance; m_cur_dist = distance; m_cur_dot = 0; }
/// Initialize this joint by specifying the two bodies to be connected and a /// joint frame specified in the absolute frame. Two local joint frames, one /// on each body, are constructed so that they coincide with the specified /// global joint frame at the current configuration. The kinematics of the /// universal joint are obtained by imposing that the origins of these two /// frames are the same and that the X axis of the joint frame on body 1 and /// the Y axis of the joint frame on body 2 are perpendicular. public void Initialize(ChBodyFrame body1, //< first body frame ChBodyFrame body2, //< second body frame ChFrame <double> frame //< joint frame (in absolute frame) ) { 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>)Body1).TransformParentToLocal(frame, m_frame1); ((ChFrame <double>)Body2).TransformParentToLocal(frame, m_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, 0.0); m_C.matrix.SetElement(1, 0, 0.0); m_C.matrix.SetElement(2, 0, 0.0); m_C.matrix.SetElement(3, 0, 0.0); }
/// Use this function after object creation, to initialize it, given /// the 1D shaft and 3D body to join. /// Each item must belong to the same ChSystem. /// Direction is expressed in the local coordinates of the body. public bool Initialize(ChShaft mshaft, //< shaft to join ChBodyFrame mbody, //< body to join ChVector mdir //< the direction of the shaft on 3D body (applied on COG: pure torque) ) { ChShaft mm1 = mshaft; ChBodyFrame mm2 = mbody; //Debug.Assert(mm1 == null && mm2 == null); shaft = mm1; body.BodyFrame = mm2; shaft_dir = ChVector.Vnorm(mdir); constraint.SetVariables(mm1.Variables(), mm2.Variables()); SetSystem(shaft.GetSystem()); return(true); }
/// Initialize this constraint, given the two bodies to be connected, the /// positions of the two anchor endpoints of the distance (each expressed /// in body or abs. coordinates) and the imposed distance. public virtual bool Initialize( ChBodyFrame mbody1, //< first frame to link ChBodyFrame mbody2, //< second frame to link bool pos_are_relative, //< true: following pos. are relative to bodies ChVector mpos1, //< pos. of distance endpoint, for 1st body (rel. or abs., see flag above) ChVector mpos2, //< pos. of distance endpoint, for 2nd body (rel. or abs., see flag above) bool auto_distance = true, //< if true, initializes the imposed distance as the distance between mpos1 and mpos2 double mdistance = 0 //< imposed distance (no need to define, if auto_distance=true.) ) { Body1 = mbody1; Body2 = mbody2; Cx.SetVariables(Body1.Variables(), Body2.Variables()); if (pos_are_relative) { pos1 = mpos1; pos2 = mpos2; } else { pos1 = Body1.TransformPointParentToLocal(mpos1); pos2 = Body2.TransformPointParentToLocal(mpos2); } ChVector AbsDist = Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2); curr_dist = AbsDist.Length(); if (auto_distance) { distance = curr_dist; } else { distance = mdistance; } return(true); }