Page MenuHomec4science
Files F65232175

sphericaljoint.cpp
No OneTemporary
Actions

File Metadata

Created
Sun, Jun 2, 01:54

sphericaljoint.cpp
View Options

/*
*_________________________________________________________________________*
* 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);
}

Event Timeline

c4science · Help