forked from lijiext/lammps
265 lines
7.7 KiB
C++
265 lines
7.7 KiB
C++
/*
|
|
*_________________________________________________________________________*
|
|
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
|
|
* DESCRIPTION: SEE READ-ME *
|
|
* FILE NAME: sphericaljoint.cpp *
|
|
* AUTHORS: See Author List *
|
|
* GRANTS: See Grants List *
|
|
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
|
|
* LICENSE: Please see License Agreement *
|
|
* DOWNLOAD: Free at www.rpi.edu/~anderk5 *
|
|
* ADMINISTRATOR: Prof. Kurt Anderson *
|
|
* Computational Dynamics Lab *
|
|
* Rensselaer Polytechnic Institute *
|
|
* 110 8th St. Troy NY 12180 *
|
|
* CONTACT: anderk5@rpi.edu *
|
|
*_________________________________________________________________________*/
|
|
|
|
|
|
#include "sphericaljoint.h"
|
|
#include "point.h"
|
|
#include "matrixfun.h"
|
|
#include "body.h"
|
|
#include "fastmatrixops.h"
|
|
#include "norm.h"
|
|
#include "eulerparameters.h"
|
|
#include "matrices.h"
|
|
#include <iomanip>
|
|
|
|
|
|
SphericalJoint::SphericalJoint(){
|
|
DimQandU(4,3);
|
|
}
|
|
SphericalJoint::~SphericalJoint(){
|
|
}
|
|
|
|
JointType SphericalJoint::GetType(){
|
|
return SPHERICALJOINT;
|
|
}
|
|
|
|
bool SphericalJoint::ReadInJointData(std::istream& in){
|
|
return true;
|
|
}
|
|
|
|
void SphericalJoint::WriteOutJointData(std::ostream& out){
|
|
}
|
|
|
|
Matrix SphericalJoint::GetForward_sP(){
|
|
Mat3x3 sPa,sPl;
|
|
Matrix sP(6,3);
|
|
sPa.Identity();
|
|
sPl.Zeros();
|
|
Vect3 temp = -(point2->position);
|
|
|
|
sPl(1,2) = temp(3);
|
|
sPl(1,3) = -temp(2);
|
|
|
|
sPl(2,1) = -temp(3);
|
|
sPl(2,3) = temp(1);
|
|
|
|
sPl(3,1) = temp(2);
|
|
sPl(3,2) = -temp(1);
|
|
|
|
sP=Stack(sPa,sPl);
|
|
return sP;
|
|
}
|
|
|
|
void SphericalJoint::UpdateForward_sP( Matrix& sP){
|
|
// sP is constant, do nothing.
|
|
// linear part is not constant
|
|
}
|
|
|
|
Matrix SphericalJoint::GetBackward_sP(){
|
|
cout<<" -----------"<<endl;
|
|
cout<<"Am I coming here "<<endl;
|
|
cout<<" -----------"<<endl;
|
|
Mat3x3 sPa,sPl;
|
|
Matrix sP;
|
|
sPa.Identity();
|
|
sPl.Zeros();
|
|
sPl(3,2)=(point2->position(1));
|
|
sPl(2,3)=-(point2->position(1));
|
|
sP=Stack(sPa,sPl);
|
|
return sP;
|
|
}
|
|
|
|
void SphericalJoint::UpdateBackward_sP( Matrix& sP){
|
|
// sP is constant, do nothing.
|
|
}
|
|
|
|
void SphericalJoint::ComputeLocalTransform(){
|
|
Mat3x3 ko_C_k;
|
|
// Obtain the transformation matrix from euler parameters
|
|
EP_Transformation(q, ko_C_k);
|
|
FastMult(pk_C_ko,ko_C_k,pk_C_k);
|
|
}
|
|
|
|
|
|
void SphericalJoint::ForwardKinematics(){
|
|
Vect3 result1,result2,result3,result4,result5;
|
|
Vect3 pk_w_k;
|
|
|
|
//cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
|
|
EP_Normalize(q);
|
|
|
|
|
|
// orientations
|
|
ComputeForwardTransforms();
|
|
|
|
|
|
//----------------------------------//
|
|
// COMPUTE POSITION VECTOR R12 aka GAMMA
|
|
|
|
FastNegMult(pk_C_k,point2->position,result1); // parents basis
|
|
FastAdd(result1,point1->position,r12);
|
|
|
|
// compute position vector r21
|
|
FastNegMult(k_C_pk,r12,r21);
|
|
|
|
|
|
|
|
//----------------------------------//
|
|
// COMPUTE GLOBAL LOCATION
|
|
FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);
|
|
FastAdd(result1,body1->r,result1);
|
|
FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);
|
|
FastAdd(result1,result2,body2->r);
|
|
|
|
qdot_to_u(q, u, qdot);
|
|
|
|
|
|
//-----------------------------------
|
|
// angular velocities
|
|
|
|
FastAssign(u,pk_w_k);
|
|
FastTMult(pk_C_k,body1->omega_k,result1);
|
|
FastAdd(result1,pk_w_k,body2->omega_k);
|
|
FastMult(body2->n_C_k,body2->omega_k,body2->omega);
|
|
|
|
|
|
|
|
//-----------------------------------
|
|
|
|
// compute velocities
|
|
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
|
|
FastAdd(body1->v_k,result1,result2);
|
|
FastTMult(pk_C_k,result2,result1); // In body basis
|
|
|
|
FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);
|
|
FastAdd(result1,result2,body2->v_k); // In body basis
|
|
FastMult(body2->n_C_k,body2->v_k,body2->v);
|
|
|
|
|
|
//------------------------------------------
|
|
//Compute the KE
|
|
Matrix tempke;
|
|
tempke = T(body2->v)*(body2->v);
|
|
double ke = 0.0;
|
|
ke = body2->mass*tempke(1,1);
|
|
FastMult(body2->inertia,body2->omega_k,result1);
|
|
tempke= T(body2->omega_k)*result1;
|
|
ke = 0.5*ke + 0.5*tempke(1,1);
|
|
body2->KE=ke;
|
|
|
|
//-----------------------------------
|
|
// compute state explicit angular acceleration // Has to be in body basis
|
|
FastTMult(pk_C_k,body1->alpha_t,result2);
|
|
FastCross(body2->omega_k,pk_w_k,result1);
|
|
FastAdd(result1,result2,body2->alpha_t);
|
|
|
|
//-----------------------------------
|
|
// compute state explicit acceleration
|
|
// NEED TO DO THIS COMPLETELY IN BODY BASIS
|
|
|
|
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
|
|
FastCross(body1->omega_k,result1,result2);
|
|
FastTMult(pk_C_k,result2,result1);
|
|
|
|
//FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);
|
|
FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);
|
|
FastCross(body2->omega_k,result3,result2);
|
|
FastAdd(result1,result2,result3); //wxwxr in body basis
|
|
|
|
FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4);
|
|
FastTMult(pk_C_k,result4,result5);
|
|
FastAssign(result5,result4);
|
|
|
|
FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);
|
|
FastAdd(result2,result4,result1); //alphaxr in body basis
|
|
|
|
FastTMult(pk_C_k,body1->a_t,result2);
|
|
FastTripleSum(result3,result1,result2,body2->a_t); // in body basis
|
|
|
|
|
|
//-----------------------------------
|
|
}
|
|
|
|
// NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
|
|
void SphericalJoint::BackwardKinematics(){
|
|
cout<<"what about here "<<endl;
|
|
Vect3 result1,result2,result3,result4,result5;
|
|
Vect3 k_w_pk;
|
|
|
|
// orientations
|
|
ComputeBackwardTransforms();
|
|
|
|
|
|
// compute position vector r21
|
|
//r21 = point2->position - k_C_pk * point1->position;
|
|
FastMult(k_C_pk,point1->position,result1);
|
|
FastSubt(point2->position,result1,r21);
|
|
|
|
|
|
// compute position vector r21
|
|
FastNegMult(pk_C_k,r21,r12);
|
|
|
|
// compute global location
|
|
// body1->r = body2->r + body2->n_C_k * r21;
|
|
FastMult(body2->n_C_k,r21,result1);
|
|
FastAdd(body2->r,result1,body1->r);
|
|
|
|
// compute qdot (for revolute joint qdot = u)
|
|
// qdot = u
|
|
ColMatrix us(3);
|
|
/*us(1)=0;
|
|
us(2)=u(1);
|
|
us(3)=u(2);*/
|
|
EP_Derivatives(q,u,qdot);
|
|
|
|
// angular velocities
|
|
|
|
FastMult(body2->n_C_k,u,result2);
|
|
FastAdd(body2->omega,result2,body1->omega);
|
|
FastAssign(u,k_w_pk);
|
|
FastMult(pk_C_k,body2->omega_k,result1);
|
|
FastSubt(result1,k_w_pk,body1->omega_k);
|
|
cout<<"The program was here"<<endl;
|
|
|
|
// compute velocities
|
|
FastCross(body2->omega_k,r21,result1);
|
|
FastCross(point1->position,k_w_pk,result2);
|
|
FastAdd(body2->v_k,result1,result3);
|
|
FastMult(pk_C_k,result3,result4);
|
|
FastAdd(result2,result4,body1->v_k);
|
|
FastMult(body1->n_C_k,body1->v_k,body1->v);
|
|
|
|
// compute state explicit angular acceleration
|
|
FastCross(body1->omega_k,k_w_pk,result1);
|
|
FastMult(pk_C_k,body2->alpha_t,result2);
|
|
FastAdd(result1,result2,body1->alpha_t);
|
|
|
|
// compute state explicit acceleration
|
|
FastCross(body2->alpha_t,point2->position,result1);
|
|
FastCross(body2->omega_k,point2->position,result2);
|
|
FastCross(body2->omega_k,result2,result3);
|
|
FastTripleSum(body2->a_t,result1,result3,result4);
|
|
FastMult(pk_C_k,result4,result5);
|
|
|
|
FastCross(point1->position,body1->alpha_t,result1);
|
|
FastCross(point1->position,body1->omega_k,result2);
|
|
FastCross(body1->omega_k,result2,result3);
|
|
|
|
FastTripleSum(result5,result1,result3,body1->a_t);
|
|
|
|
}
|