Page MenuHomec4science
Files F65948558

prismaticjoint.cpp
No OneTemporary
Actions

File Metadata

Created
Fri, Jun 7, 07:05

prismaticjoint.cpp
View Options

/*
*_________________________________________________________________________*
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: prismaticjoint.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 "prismaticjoint.h"
#include "point.h"
#include "matrixfun.h"
#include "body.h"
#include "fastmatrixops.h"
PrismaticJoint::PrismaticJoint(){
q.Dim(1);
qdot.Dim(1);
u.Dim(1);
udot.Dim(1);
}
PrismaticJoint::~PrismaticJoint(){
}
JointType PrismaticJoint::GetType(){
return PRISMATICJOINT;
}
bool PrismaticJoint::ReadInJointData(std::istream& in){
in >> axis_pk;
axis_k = T(pk_C_ko)*axis_pk;
// init the constant transforms
pk_C_k = pk_C_ko;
k_C_pk = T(pk_C_k);
return true;
}
void PrismaticJoint::WriteOutJointData(std::ostream& out){
out << axis_pk;
}
Matrix PrismaticJoint::GetForward_sP(){
Vect3 zero;
zero.Zeros();
// sP = [zero;axis]
return Stack(zero,axis_k);
}
void PrismaticJoint::UpdateForward_sP( Matrix& sP){
// sP is constant, do nothing.
}
Matrix PrismaticJoint::GetBackward_sP(){
Vect3 zero;
zero.Zeros();
// sP = [zero;axis]
return -Stack(zero,axis_pk);
}
void PrismaticJoint::UpdateBackward_sP( Matrix& sP){
// sP is constant, do nothing.
}
void PrismaticJoint::ComputeForwardTransforms(){
ComputeForwardGlobalTransform();
}
void PrismaticJoint::ComputeBackwardTransforms(){
ComputeBackwardGlobalTransform();
}
void PrismaticJoint::ComputeLocalTransform(){
// the transform is constant, do nothing
}
void PrismaticJoint::ForwardKinematics(){
Vect3 result1,result2,result3;
Vect3 d_pk;
// orientations
ComputeForwardTransforms();
// compute position vector r12
//r12 = point1->position + axis_pk * q - pk_C_k * point2->position;
FastMult(pk_C_k,point2->position,result1);
FastMult(q.BasicGet(0),axis_pk,d_pk);
FastTripleSumPPM(point1->position,d_pk,result1,r12);
// compute position vector r21
FastNegMult(k_C_pk,r12,r21);
// compute global location
// body2->r = body1->r + body1->n_C_k * r12;
FastMult(body1->n_C_k,r12,result1);
FastAdd(body1->r,result1,body2->r);
// compute qdot (for Prismatic joint qdot = u)
// qdot = u
FastAssign(u,qdot);
// angular velocities
//body2->omega = body1->omega;
//body2->omega_k = T(pk_C_k) * body1->omega_k;
FastAssign(body1->omega,body2->omega);
FastMult(k_C_pk,body1->omega_k,body2->omega_k);
// compute velocities
Vect3 pk_v_k;
Vect3 wxgamma;
FastMult(u.BasicGet(0),axis_k,pk_v_k);
FastMult(k_C_pk,body1->v_k,result1);
FastCross(body2->omega_k,r12,wxgamma);
FastTripleSum(result1,pk_v_k,wxgamma,body2->v_k);
FastMult(body2->n_C_k,body2->v_k,body2->v);
// compute state explicit angular acceleration
FastMult(k_C_pk,body1->alpha_t,body2->alpha_t);
// compute state explicit acceleration
FastCross(r21,body1->alpha_t,result1);
FastAdd(body1->a_t,result1,result2);
FastMult(k_C_pk,result2,result1);
FastCross(body2->omega_k,pk_v_k,result2);
FastMult(2.0,result2,result3);
FastCross(body2->omega_k,wxgamma,result2);
FastTripleSum(result1,result2,result3,body2->a_t);
}
void PrismaticJoint::BackwardKinematics(){
Vect3 result1,result2,result3;
Vect3 d_k;
// orientations
ComputeBackwardTransforms();
// compute position vector r21
//r21 = point2->position + axis_k * q - k_C_pk * point1->position;
FastMult(k_C_pk,point1->position,result1);
FastMult(-q.BasicGet(0),axis_k,d_k);
FastTripleSumPPM(point2->position,d_k,result1,r21);
// compute position vector r12
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 Prismatic joint qdot = u)
// qdot = u
FastAssign(u,qdot);
// angular velocities
//body1->omega = body2->omega;
//body1->omega_k = pk_C_k * body2->omega_k;
FastAssign(body2->omega,body1->omega);
FastMult(pk_C_k,body2->omega_k,body1->omega_k);
// compute velocities
Vect3 k_v_pk;
Vect3 wxgamma;
FastMult(-u.BasicGet(0),axis_pk,k_v_pk);
FastMult(pk_C_k,body2->v_k,result1);
FastCross(body1->omega_k,r21,wxgamma);
FastTripleSum(result1,k_v_pk,wxgamma,body1->v_k);
FastMult(body1->n_C_k,body1->v_k,body1->v);
// compute state explicit angular acceleration
FastMult(pk_C_k,body2->alpha_t,body1->alpha_t);
// compute state explicit acceleration
FastCross(r12,body2->alpha_t,result1);
FastAdd(body2->a_t,result1,result2);
FastMult(pk_C_k,result2,result1);
FastCross(body1->omega_k,k_v_pk,result2);
FastMult(2.0,result2,result3);
FastCross(body1->omega_k,wxgamma,result2);
FastTripleSum(result1,result2,result3,body1->a_t);
}

Event Timeline

c4science · Help