00001 #ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_JOINTS_MBD_UNIVERSAL_JOINT_H
00002 #define OPENTISSUE_DYNAMICS_MBD_UTIL_JOINTS_MBD_UNIVERSAL_JOINT_H
00003
00004
00005
00006
00007
00008
00009
00010 #include <OpenTissue/configuration.h>
00011
00012 #include <OpenTissue/dynamics/mbd/interfaces/mbd_joint_interface.h>
00013 #include <OpenTissue/dynamics/mbd/limits/mbd_angular_joint_limit.h>
00014 #include <OpenTissue/dynamics/mbd/motors/mbd_angular_joint_motor.h>
00015 #include <OpenTissue/core/math/math_constants.h>
00016
00017
00018 namespace OpenTissue
00019 {
00020 namespace mbd
00021 {
00022
00023 template < typename mbd_types >
00024 class UniversalJoint
00025 : public JointInterface<mbd_types>
00026 {
00027 public:
00028
00029 typedef typename mbd_types::math_policy math_policy;
00030 typedef typename mbd_types::math_policy::index_type size_type;
00031 typedef typename mbd_types::math_policy::real_type real_type;
00032 typedef typename mbd_types::math_policy::vector3_type vector3_type;
00033 typedef typename mbd_types::math_policy::matrix3x3_type matrix3x3_type;
00034 typedef typename mbd_types::math_policy::quaternion_type quaternion_type;
00035 typedef typename mbd_types::math_policy::vector_range vector_range;
00036 typedef typename mbd_types::math_policy::idx_vector_range idx_vector_range;
00037 typedef typename mbd_types::math_policy::matrix_range matrix_range;
00038 typedef typename mbd_types::math_policy::vector_type vector_type;
00039 typedef typename mbd_types::math_policy::value_traits value_traits;
00040
00041 typedef AngularJointLimit<mbd_types> angular_limit_type;
00042 typedef AngularJointMotor<mbd_types> angular_motor_type;
00043
00044 protected:
00045
00046 vector3_type m_s_A_wcs;
00047 vector3_type m_s_B_wcs;
00048 vector3_type m_u;
00049 matrix3x3_type m_star_anc_A;
00050 matrix3x3_type m_star_anc_B;
00051 vector3_type m_anc_A_wcs;
00052 vector3_type m_anc_B_wcs;
00053 quaternion_type m_Q_initial;
00054 quaternion_type m_Q_initial_conj;
00055 size_type m_rows;
00056 vector_type m_gamma;
00057 vector_type m_solution;
00058
00059 angular_limit_type * m_limit1;
00060 angular_limit_type * m_limit2;
00061 angular_motor_type * m_motor1;
00062 angular_motor_type * m_motor2;
00063
00064 size_type m_motor_offset1;
00065 size_type m_motor_offset2;
00066 size_type m_limit_offset1;
00067 size_type m_limit_offset2;
00068
00069 public:
00070
00075 real_type get_angle1() const
00076 {
00077 assert( this->m_socketA || !"UniversalJoint::get_angle1(): socket A was null");
00078 assert( this->m_socketB || !"UniversalJoint::get_angle1(): socket B was null");
00079
00080 quaternion_type Q_A, Q_B;
00081
00082 this->m_socketA->get_body() ->get_orientation( Q_A );
00083 this->m_socketB->get_body() ->get_orientation( Q_B );
00084
00085 quaternion_type Q_rel = ( conj( Q_B ) % Q_A ) % m_Q_initial_conj;
00086 return get_angle( Q_rel, conj( Q_B ).rotate( get_joint_axis1_world() ) );
00087 }
00088
00096 real_type get_angle1_rate() const
00097 {
00098 assert( this->m_socketA || !"UniversalJoint::get_angle1_rate(): socket A was null");
00099 assert( this->m_socketB || !"UniversalJoint::get_angle1_rate(): socket B was null");
00100
00101 this->m_socketA->get_body() ->get_spin( this->w_A );
00102 this->m_socketB->get_body() ->get_spin( this->w_B );
00103
00104 return get_joint_axis1_world() * ( this->w_A - this->w_B );
00105 }
00106
00111 real_type get_angle2() const
00112 {
00113 assert( this->m_socketA || !"UniversalJoint::get_angle2(): socket A was null");
00114 assert( this->m_socketB || !"UniversalJoint::get_angle2(): socket B was null");
00115
00116 quaternion_type Q_A, Q_B;
00117
00118 this->m_socketA->get_body() ->get_orientation( Q_A );
00119 this->m_socketB->get_body() ->get_orientation( Q_B );
00120
00121 quaternion_type Q_rel = ( conj( Q_B ) % Q_A ) % m_Q_initial_conj;
00122
00123 return get_angle( Q_rel, conj( Q_B ).rotate( get_joint_axis2_world() ) );
00124 }
00125
00133 real_type get_angle2_rate() const
00134 {
00135 assert( this->m_socketA || !"UniversalJoint::get_angle2_rate(): socket A was null");
00136 assert( this->m_socketB || !"UniversalJoint::get_angle2_rate(): socket B was null");
00137
00138 this->m_socketA->get_body() ->get_spin( this->w_A );
00139 this->m_socketB->get_body() ->get_spin( this->w_B );
00140 return get_joint_axis2_world() * ( this->w_A - this->w_B );
00141 }
00142
00143 vector3_type get_joint_axis1_world() const
00144 {
00145 return this->m_socketA->get_joint_axis_world();
00146 }
00147
00148 vector3_type get_joint_axis2_world() const
00149 {
00150 return this->m_socketB->get_joint_axis_world();
00151 }
00152
00153 void set_limit1( angular_limit_type const & limit )
00154 {
00155 m_limit1 = const_cast<angular_limit_type*>( &limit );
00156 }
00157
00158 void set_limit2( angular_limit_type const & limit )
00159 {
00160 m_limit2 = const_cast<angular_limit_type*>( &limit );
00161 }
00162
00163 void set_motor1( angular_motor_type const & motor )
00164 {
00165 m_motor1 = const_cast<angular_motor_type*>( &motor );
00166 }
00167
00168 void set_motor2( angular_motor_type const & motor )
00169 {
00170 m_motor2 = const_cast<angular_motor_type*>( &motor );
00171 }
00172
00173 public:
00174
00175 UniversalJoint()
00176 : m_rows( 0 )
00177 , m_limit1( 0 )
00178 , m_limit2( 0 )
00179 , m_motor1( 0 )
00180 , m_motor2( 0 )
00181 {}
00182
00183 virtual ~UniversalJoint(){}
00184
00185 public:
00186
00187 void evaluate()
00188 {
00189 m_rows = 0;
00190
00191 if ( !( this->m_socketA && this->m_socketB ) )
00192 return ;
00193
00194 m_rows = 4;
00195
00196 m_s_A_wcs = this->m_socketA->get_joint_axis_world();
00197 m_s_B_wcs = this->m_socketB->get_joint_axis_world();
00198
00199 quaternion_type Q;
00200 this->m_socketA->get_body() ->get_orientation( Q );
00201 m_star_anc_A = star( Q.rotate( this->m_socketA->get_anchor_local() ) );
00202 this->m_socketB->get_body() ->get_orientation( Q );
00203 m_star_anc_B = star( Q.rotate( this->m_socketB->get_anchor_local() ) );
00204
00205 m_anc_A_wcs = this->m_socketA->get_anchor_world();
00206 m_anc_B_wcs = this->m_socketB->get_anchor_world();
00207
00208 m_u = cross(m_s_A_wcs , m_s_B_wcs);
00209
00210 m_limit_offset1 = 0;
00211 m_motor_offset1 = 0;
00212 if ( m_limit1 )
00213 {
00214 m_limit1->evaluate( get_joint_axis1_world(), get_angle1() );
00215 if ( m_limit1->is_active() )
00216 {
00217 m_limit_offset1 = m_rows;
00218 ++m_rows;
00219 }
00220 }
00221 if ( m_motor1 )
00222 {
00223 m_motor1->evaluate( get_joint_axis1_world() );
00224 if ( m_motor1->is_active() )
00225 {
00226 m_motor_offset1 = m_rows;
00227 ++m_rows;
00228 }
00229 }
00230 m_limit_offset2 = 0;
00231 m_motor_offset2 = 0;
00232 if ( m_limit2 )
00233 {
00234 m_limit2->evaluate( get_joint_axis2_world(), get_angle2() );
00235 if ( m_limit2->is_active() )
00236 {
00237 m_limit_offset2 = m_rows;
00238 ++m_rows;
00239 }
00240 }
00241 if ( m_motor2 )
00242 {
00243 m_motor2->evaluate( get_joint_axis2_world() );
00244 if ( m_motor2->is_active() )
00245 {
00246 m_motor_offset2 = m_rows;
00247 ++m_rows;
00248 }
00249 }
00250 }
00251
00252 size_type get_number_of_jacobian_rows() const
00253 {
00254 return m_rows;
00255 }
00256
00257 void get_linear_jacobian_A( matrix_range & J ) const
00258 {
00259 assert( J.size1() == m_rows || !"UniversalJoint::get_linear_jacobian_A(): incorrect dimensions");
00260 assert( J.size2() == 3 || !"UniversalJoint::get_linear_jacobian_A(): incorrect dimensions");
00261 J( 0, 0 ) = value_traits::one();
00262 J( 0, 1 ) = value_traits::zero();
00263 J( 0, 2 ) = value_traits::zero();
00264 J( 1, 0 ) = value_traits::zero();
00265 J( 1, 1 ) = value_traits::one();
00266 J( 1, 2 ) = value_traits::zero();
00267 J( 2, 0 ) = value_traits::zero();
00268 J( 2, 1 ) = value_traits::zero();
00269 J( 2, 2 ) = value_traits::one();
00270 J( 3, 0 ) = value_traits::zero();
00271 J( 3, 1 ) = value_traits::zero();
00272 J( 3, 2 ) = value_traits::zero();
00273
00274 if ( m_limit_offset1 )
00275 m_limit1->get_linear_jacobian_A( J, m_limit_offset1 );
00276 if ( m_motor_offset1 )
00277 m_motor1->get_linear_jacobian_A( J, m_motor_offset1 );
00278 if ( m_limit_offset2 )
00279 m_limit2->get_linear_jacobian_A( J, m_limit_offset2 );
00280 if ( m_motor_offset2 )
00281 m_motor2->get_linear_jacobian_A( J, m_motor_offset2 );
00282 }
00283
00284 void get_linear_jacobian_B( matrix_range & J ) const
00285 {
00286 assert( J.size1() == m_rows || !"UniversalJoint::get_linear_jacobian_B(): incorrect dimensions");
00287 assert( J.size2() == 3 || !"UniversalJoint::get_linear_jacobian_B(): incorrect dimensions");
00288 J( 0, 0 ) = -value_traits::one();
00289 J( 0, 1 ) = value_traits::zero();
00290 J( 0, 2 ) = value_traits::zero();
00291 J( 1, 0 ) = value_traits::zero();
00292 J( 1, 1 ) = -value_traits::one();
00293 J( 1, 2 ) = value_traits::zero();
00294 J( 2, 0 ) = value_traits::zero();
00295 J( 2, 1 ) = value_traits::zero();
00296 J( 2, 2 ) = -value_traits::one();
00297 J( 3, 0 ) = value_traits::zero();
00298 J( 3, 1 ) = value_traits::zero();
00299 J( 3, 2 ) = value_traits::zero();
00300
00301 if ( m_limit_offset1 )
00302 m_limit1->get_linear_jacobian_B( J, m_limit_offset1 );
00303 if ( m_motor_offset1 )
00304 m_motor1->get_linear_jacobian_B( J, m_motor_offset1 );
00305 if ( m_limit_offset2 )
00306 m_limit2->get_linear_jacobian_B( J, m_limit_offset2 );
00307 if ( m_motor_offset2 )
00308 m_motor2->get_linear_jacobian_B( J, m_motor_offset2 );
00309 }
00310
00311 void get_angular_jacobian_A( matrix_range & J ) const
00312 {
00313 assert( J.size1() == m_rows || !"UniversalJoint::get_angular_jacobian_A(): incorrect dimensions");
00314 assert( J.size2() == 3 || !"UniversalJoint::get_angular_jacobian_A(): incorrect dimensions");
00315 J( 0, 0 ) = -m_star_anc_A( 0, 0 );
00316 J( 0, 1 ) = -m_star_anc_A( 0, 1 );
00317 J( 0, 2 ) = -m_star_anc_A( 0, 2 );
00318 J( 1, 0 ) = -m_star_anc_A( 1, 0 );
00319 J( 1, 1 ) = -m_star_anc_A( 1, 1 );
00320 J( 1, 2 ) = -m_star_anc_A( 1, 2 );
00321 J( 2, 0 ) = -m_star_anc_A( 2, 0 );
00322 J( 2, 1 ) = -m_star_anc_A( 2, 1 );
00323 J( 2, 2 ) = -m_star_anc_A( 2, 2 );
00324 J( 3, 0 ) = m_u( 0 );
00325 J( 3, 1 ) = m_u( 1 );
00326 J( 3, 2 ) = m_u( 2 );
00327 if ( m_limit_offset1 )
00328 m_limit1->get_angular_jacobian_A( J, m_limit_offset1 );
00329 if ( m_motor_offset1 )
00330 m_motor1->get_angular_jacobian_A( J, m_motor_offset1 );
00331 if ( m_limit_offset2 )
00332 m_limit2->get_angular_jacobian_A( J, m_limit_offset2 );
00333 if ( m_motor_offset2 )
00334 m_motor2->get_angular_jacobian_A( J, m_motor_offset2 );
00335 }
00336
00337 void get_angular_jacobian_B( matrix_range & J ) const
00338 {
00339 assert( J.size1() == m_rows || !"UniversalJoint::get_angular_jacobian_B(): incorrect dimensions");
00340 assert( J.size2() == 3 || !"UniversalJoint::get_angular_jacobian_B(): incorrect dimensions");
00341 J( 0, 0 ) = m_star_anc_B( 0, 0 );
00342 J( 0, 1 ) = m_star_anc_B( 0, 1 );
00343 J( 0, 2 ) = m_star_anc_B( 0, 2 );
00344 J( 1, 0 ) = m_star_anc_B( 1, 0 );
00345 J( 1, 1 ) = m_star_anc_B( 1, 1 );
00346 J( 1, 2 ) = m_star_anc_B( 1, 2 );
00347 J( 2, 0 ) = m_star_anc_B( 2, 0 );
00348 J( 2, 1 ) = m_star_anc_B( 2, 1 );
00349 J( 2, 2 ) = m_star_anc_B( 2, 2 );
00350 J( 3, 0 ) = -m_u( 0 );
00351 J( 3, 1 ) = -m_u( 1 );
00352 J( 3, 2 ) = -m_u( 2 );
00353 if ( m_limit_offset1 )
00354 m_limit1->get_angular_jacobian_B( J, m_limit_offset1 );
00355 if ( m_motor_offset1 )
00356 m_motor1->get_angular_jacobian_B( J, m_motor_offset1 );
00357 if ( m_limit_offset2 )
00358 m_limit2->get_angular_jacobian_B( J, m_limit_offset2 );
00359 if ( m_motor_offset2 )
00360 m_motor2->get_angular_jacobian_B( J, m_motor_offset2 );
00361 }
00362
00363 void get_stabilization_term( vector_range & b_error ) const
00364 {
00365 assert( b_error.size() == m_rows || !"UniversalJoint::get_stabilization_term(): incorrect dimensions");
00366
00367 real_type k_cor = this->get_frames_per_second()*this->get_error_reduction_parameter();
00368
00369 b_error( 0 ) = k_cor * ( m_anc_B_wcs( 0 ) - m_anc_A_wcs( 0 ) );
00370 b_error( 1 ) = k_cor * ( m_anc_B_wcs( 1 ) - m_anc_A_wcs( 1 ) );
00371 b_error( 2 ) = k_cor * ( m_anc_B_wcs( 2 ) - m_anc_A_wcs( 2 ) );
00372 b_error( 3 ) = k_cor * ( -m_s_A_wcs * m_s_B_wcs );
00373 if ( m_limit_offset1 )
00374 m_limit1->get_stabilization_term( b_error, m_limit_offset1 );
00375 if ( m_motor_offset1 )
00376 m_motor1->get_stabilization_term( b_error, m_motor_offset1 );
00377 if ( m_limit_offset2 )
00378 m_limit2->get_stabilization_term( b_error, m_limit_offset2 );
00379 if ( m_motor_offset2 )
00380 m_motor2->get_stabilization_term( b_error, m_motor_offset2 );
00381 }
00382
00383 void get_low_limits( vector_range & lo ) const
00384 {
00385 assert( lo.size() == m_rows || !"UniversalJoint::get_low_limits(): incorrect dimensions");
00386
00387 lo(0) = OpenTissue::math::detail::lowest<real_type>();
00388 lo(1) = OpenTissue::math::detail::lowest<real_type>();
00389 lo(2) = OpenTissue::math::detail::lowest<real_type>();
00390 lo(3) = OpenTissue::math::detail::lowest<real_type>();
00391 if ( m_limit_offset1 )
00392 m_limit1->get_low_limits( lo, m_limit_offset1 );
00393 if ( m_motor_offset1 )
00394 m_motor1->get_low_limits( lo, m_motor_offset1 );
00395 if ( m_limit_offset2 )
00396 m_limit2->get_low_limits( lo, m_limit_offset2 );
00397 if ( m_motor_offset2 )
00398 m_motor2->get_low_limits( lo, m_motor_offset2 );
00399 }
00400
00401 void get_high_limits( vector_range & hi ) const
00402 {
00403 assert( hi.size() == m_rows || !"UniversalJoint::get_high_limits(): incorrect dimensions");
00404
00405 hi(0) = OpenTissue::math::detail::highest<real_type>();
00406 hi(1) = OpenTissue::math::detail::highest<real_type>();
00407 hi(2) = OpenTissue::math::detail::highest<real_type>();
00408 hi(3) = OpenTissue::math::detail::highest<real_type>();
00409 if ( m_limit_offset1 )
00410 m_limit1->get_high_limits( hi, m_limit_offset1 );
00411 if ( m_motor_offset1 )
00412 m_motor1->get_high_limits( hi, m_motor_offset1 );
00413 if ( m_limit_offset2 )
00414 m_limit2->get_high_limits( hi, m_limit_offset2 );
00415 if ( m_motor_offset2 )
00416 m_motor2->get_high_limits( hi, m_motor_offset2 );
00417 }
00418
00419 void get_dependency_indices( idx_vector_range & dep ) const
00420 {
00421 assert( dep.size() == m_rows || !"UniversalJoint::get_dependency_indices(): incorrect dimensions");
00422 for ( size_type i = 0;i < m_rows;++i )
00423 dep( i ) = OpenTissue::math::detail::highest<size_type>();
00424 }
00425
00426 void get_dependency_factors( vector_range & factors ) const
00427 {
00428 assert( factors.size() == m_rows || !"UniversalJoint::get_dependency_factors(): incorrect dimensions");
00429 for ( size_type i = 0;i < m_rows;++i )
00430 factors( i ) = value_traits::zero();
00431 }
00432
00433 void set_regularization( vector_range const & gamma )
00434 {
00435 assert( gamma.size() == m_rows || !"UniversalJoint::set_regularization(): incorrect dimensions");
00436 math_policy::resize( m_gamma, 4 );
00437 for ( size_type i = 0;i < 4;++i )
00438 {
00439 assert( gamma( i ) <= value_traits::one() || !"UniversalJoint::set_regularization(): gamma was greater than 1");
00440 assert( gamma( i ) >= value_traits::zero() || !"UniversalJoint::set_regularization(): gamma was less than 0");
00441 m_gamma( i ) = gamma( i );
00442 }
00443 if ( m_limit_offset1 )
00444 m_limit1->set_regularization( gamma, m_limit_offset1 );
00445 if ( m_motor_offset1 )
00446 m_motor1->set_regularization( gamma, m_motor_offset1 );
00447 if ( m_limit_offset2 )
00448 m_limit2->set_regularization( gamma, m_limit_offset2 );
00449 if ( m_motor_offset2 )
00450 m_motor2->set_regularization( gamma, m_motor_offset2 );
00451 }
00452
00453 void get_regularization( vector_range & gamma ) const
00454 {
00455 assert( gamma.size() == m_rows || !"UniversalJoint::get_regularization(): incorrect dimensions");
00456 if ( m_gamma.size() == 0 )
00457 {
00458 for ( size_type i = 0;i < 4;++i )
00459 gamma( i ) = value_traits::zero();
00460 }
00461 else
00462 {
00463 for ( size_type i = 0;i < 4;++i )
00464 gamma( i ) = m_gamma( i );
00465 }
00466 if ( m_limit_offset1 )
00467 m_limit1->get_regularization( gamma, m_limit_offset1 );
00468 if ( m_motor_offset1 )
00469 m_motor1->get_regularization( gamma, m_motor_offset1 );
00470 if ( m_limit_offset2 )
00471 m_limit2->get_regularization( gamma, m_limit_offset2 );
00472 if ( m_motor_offset2 )
00473 m_motor2->get_regularization( gamma, m_motor_offset2 );
00474 }
00475
00476 void set_solution( vector_range const & solution )
00477 {
00478 assert( solution.size() == m_rows || !"UniversalJoint::set_solution(): incorrect dimensions");
00479 math_policy::resize( m_solution, 4 );
00480 for ( size_type i = 0;i < 4;++i )
00481 m_solution( i ) = solution( i );
00482 if ( m_limit_offset1 )
00483 m_limit1->set_solution( solution, m_limit_offset1 );
00484 if ( m_motor_offset1 )
00485 m_motor1->set_solution( solution, m_motor_offset1 );
00486 if ( m_limit_offset2 )
00487 m_limit2->set_solution( solution, m_limit_offset2 );
00488 if ( m_motor_offset2 )
00489 m_motor2->set_solution( solution, m_motor_offset2 );
00490 }
00491
00492 void get_solution( vector_range & solution ) const
00493 {
00494 assert( solution.size() == m_rows || !"UniversalJoint::get_solution(): incorrect dimensions");
00495 if ( m_solution.size() == 0 )
00496 {
00497 for ( size_type i = 0;i < 4;++i )
00498 solution( i ) = value_traits::zero();
00499 }
00500 else
00501 {
00502 for ( size_type i = 0;i < 4;++i )
00503 solution( i ) = m_solution( i );
00504 }
00505 if ( m_limit_offset1 )
00506 m_limit1->get_solution( solution, m_limit_offset1 );
00507 if ( m_motor_offset1 )
00508 m_motor1->get_solution( solution, m_motor_offset1 );
00509 if ( m_limit_offset2 )
00510 m_limit2->get_solution( solution, m_limit_offset2 );
00511 if ( m_motor_offset2 )
00512 m_motor2->get_solution( solution, m_motor_offset2 );
00513 }
00514
00515 void calibration()
00516 {
00517 using std::fabs;
00518
00519 assert( this->m_socketA || !"UniversalJoint::calibration(): socket A was null");
00520 assert( this->m_socketB || !"UniversalJoint::calibration(): socket B was null");
00521
00522 quaternion_type Q_A, Q_B;
00523 this->m_socketA->get_body() ->get_orientation( Q_A );
00524 this->m_socketB->get_body() ->get_orientation( Q_B );
00525
00526 m_Q_initial = conj( Q_B ) % Q_A;
00527 m_Q_initial_conj = conj( m_Q_initial );
00528
00529
00530 vector3_type s_A_wcs = this->m_socketA->get_joint_axis_world();
00531 vector3_type s_B_wcs = this->m_socketB->get_joint_axis_world();
00532
00533 assert( fabs( s_A_wcs * s_B_wcs ) < 10e-7 || !"UniversalJoint::calibration(): axes where orthogonal");
00534 }
00535
00536 };
00537
00538 }
00539 }
00540
00541 #endif
