1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora
13 // =============================================================================
14
15 #include "chrono/physics/ChShaftsMotorAngle.h"
16
17 namespace chrono {
18
19
20 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChShaftsMotorAngle)21 CH_FACTORY_REGISTER(ChShaftsMotorAngle)
22
23 ChShaftsMotorAngle::ChShaftsMotorAngle() : rot_offset(0), violation(0), motor_torque(0) {
24 // default motion function : a ramp
25 this->f_rot = chrono_types::make_shared<ChFunction_Ramp>(
26 0.0, // default y(0)
27 1.0 // default dy/dx , i.e. 1 [rad/s]
28 );
29
30 }
31
ChShaftsMotorAngle(const ChShaftsMotorAngle & other)32 ChShaftsMotorAngle::ChShaftsMotorAngle(const ChShaftsMotorAngle& other)
33 : ChShaftsMotorBase(other), violation(0), motor_torque(0) {
34 this->f_rot = other.f_rot;
35 this->rot_offset = other.rot_offset;
36 }
37
Initialize(std::shared_ptr<ChShaft> mshaft1,std::shared_ptr<ChShaft> mshaft2)38 bool ChShaftsMotorAngle::Initialize(std::shared_ptr<ChShaft> mshaft1, std::shared_ptr<ChShaft> mshaft2) {
39 // Parent class initialize
40 if (!ChShaftsMotorBase::Initialize(mshaft1, mshaft2))
41 return false;
42
43 ChShaft* mm1 = mshaft1.get();
44 ChShaft* mm2 = mshaft2.get();
45
46 constraint.SetVariables(&mm1->Variables(), &mm2->Variables());
47
48 SetSystem(shaft1->GetSystem());
49
50 return true;
51 }
52
Update(double mytime,bool update_assets)53 void ChShaftsMotorAngle::Update(double mytime, bool update_assets) {
54 // Inherit time changes of parent class
55 ChShaftsMotorBase::Update(mytime, update_assets);
56
57 // Update class data
58 this->f_rot->Update(mytime); // call callbacks if any
59 violation = GetMotorRot() - f_rot->Get_y(mytime) - rot_offset;
60 }
61
62 //// STATE BOOKKEEPING FUNCTIONS
63
IntStateGatherReactions(const unsigned int off_L,ChVectorDynamic<> & L)64 void ChShaftsMotorAngle::IntStateGatherReactions(const unsigned int off_L, ChVectorDynamic<>& L) {
65 L(off_L) = motor_torque;
66 }
67
IntStateScatterReactions(const unsigned int off_L,const ChVectorDynamic<> & L)68 void ChShaftsMotorAngle::IntStateScatterReactions(const unsigned int off_L, const ChVectorDynamic<>& L) {
69 motor_torque = L(off_L);
70 }
71
IntLoadResidual_CqL(const unsigned int off_L,ChVectorDynamic<> & R,const ChVectorDynamic<> & L,const double c)72 void ChShaftsMotorAngle::IntLoadResidual_CqL(const unsigned int off_L, // offset in L multipliers
73 ChVectorDynamic<>& R, // result: the R residual, R += c*Cq'*L
74 const ChVectorDynamic<>& L, // the L vector
75 const double c // a scaling factor
76 ) {
77 constraint.MultiplyTandAdd(R, L(off_L) * c);
78 }
79
IntLoadConstraint_C(const unsigned int off_L,ChVectorDynamic<> & Qc,const double c,bool do_clamp,double recovery_clamp)80 void ChShaftsMotorAngle::IntLoadConstraint_C(const unsigned int off_L, // offset in Qc residual
81 ChVectorDynamic<>& Qc, // result: the Qc residual, Qc += c*C
82 const double c, // a scaling factor
83 bool do_clamp, // apply clamping to c*C?
84 double recovery_clamp // value for min/max clamping of c*C
85 ) {
86 double res = c * (GetMotorRot() - this->f_rot->Get_y(this->GetChTime()) - this->rot_offset);
87
88 if (do_clamp) {
89 res = ChMin(ChMax(res, -recovery_clamp), recovery_clamp);
90 }
91 Qc(off_L) += res;
92 }
93
IntLoadConstraint_Ct(const unsigned int off_L,ChVectorDynamic<> & Qc,const double c)94 void ChShaftsMotorAngle::IntLoadConstraint_Ct(const unsigned int off_L, // offset in Qc residual
95 ChVectorDynamic<>& Qc, // result: the Qc residual, Qc += c*Ct
96 const double c // a scaling factor
97 ) {
98 double ct = - this->f_rot->Get_y_dx(this->GetChTime());
99 Qc(off_L) += c * ct;
100 }
101
IntToDescriptor(const unsigned int off_v,const ChStateDelta & v,const ChVectorDynamic<> & R,const unsigned int off_L,const ChVectorDynamic<> & L,const ChVectorDynamic<> & Qc)102 void ChShaftsMotorAngle::IntToDescriptor(const unsigned int off_v, // offset in v, R
103 const ChStateDelta& v,
104 const ChVectorDynamic<>& R,
105 const unsigned int off_L, // offset in L, Qc
106 const ChVectorDynamic<>& L,
107 const ChVectorDynamic<>& Qc) {
108 constraint.Set_l_i(L(off_L));
109 constraint.Set_b_i(Qc(off_L));
110 }
111
IntFromDescriptor(const unsigned int off_v,ChStateDelta & v,const unsigned int off_L,ChVectorDynamic<> & L)112 void ChShaftsMotorAngle::IntFromDescriptor(const unsigned int off_v, // offset in v
113 ChStateDelta& v,
114 const unsigned int off_L, // offset in L
115 ChVectorDynamic<>& L) {
116 L(off_L) = constraint.Get_l_i();
117 }
118
119 // SOLVER INTERFACES
120
InjectConstraints(ChSystemDescriptor & mdescriptor)121 void ChShaftsMotorAngle::InjectConstraints(ChSystemDescriptor& mdescriptor) {
122
123 mdescriptor.InsertConstraint(&constraint);
124 }
125
ConstraintsBiReset()126 void ChShaftsMotorAngle::ConstraintsBiReset() {
127
128 constraint.Set_b_i(0.);
129 }
130
ConstraintsBiLoad_C(double factor,double recovery_clamp,bool do_clamp)131 void ChShaftsMotorAngle::ConstraintsBiLoad_C(double factor, double recovery_clamp, bool do_clamp) {
132
133 double res = factor * (GetMotorRot() - this->f_rot->Get_y(this->GetChTime()) - this->rot_offset);
134
135 constraint.Set_b_i(constraint.Get_b_i() + factor * res);
136 }
137
ConstraintsBiLoad_Ct(double factor)138 void ChShaftsMotorAngle::ConstraintsBiLoad_Ct(double factor) {
139
140 double ct = - this->f_rot->Get_y_dx(this->GetChTime());
141 constraint.Set_b_i(constraint.Get_b_i() + factor * ct);
142 }
143
ConstraintsLoadJacobians()144 void ChShaftsMotorAngle::ConstraintsLoadJacobians() {
145 constraint.Get_Cq_a()(0) = 1;
146 constraint.Get_Cq_b()(0) = -1;
147 }
148
ConstraintsFetch_react(double factor)149 void ChShaftsMotorAngle::ConstraintsFetch_react(double factor) {
150 motor_torque = - constraint.Get_l_i() * factor;
151 }
152
153 //////// FILE I/O
154
ArchiveOUT(ChArchiveOut & marchive)155 void ChShaftsMotorAngle::ArchiveOUT(ChArchiveOut& marchive) {
156 // version number
157 marchive.VersionWrite<ChShaftsMotorAngle>();
158
159 // serialize parent class
160 ChShaftsMotorBase::ArchiveOUT(marchive);
161
162 // serialize all member data:
163 marchive << CHNVP(motor_torque);
164 marchive << CHNVP(this->rot_offset);
165 marchive << CHNVP(this->f_rot);
166
167 }
168
169 /// Method to allow de serialization of transient data from archives.
ArchiveIN(ChArchiveIn & marchive)170 void ChShaftsMotorAngle::ArchiveIN(ChArchiveIn& marchive) {
171 // version number
172 /*int version =*/ marchive.VersionRead<ChShaftsMotorAngle>();
173
174 // deserialize parent class:
175 ChShaftsMotorBase::ArchiveIN(marchive);
176
177 // deserialize all member data:
178 marchive >> CHNVP(motor_torque);
179 marchive >> CHNVP(this->rot_offset);
180 marchive >> CHNVP(this->f_rot);
181 }
182
183
184
185
186 } // end namespace chrono
187