Constraint on free body

[Andrew Hamilton]

I am trying to include a body with prescribed motion and have implemented this as a free body with constraints on all six dof. This has allowed me to write my own functions with the prescribed motion in each degree of freedom. This works fine, for translational dof's I get what I expect but for rotational modes, the resulting motion is just slightly less than twice what the function specifies. I've reduced this to the simplest program I can (below). In this I ask it to rotate in a prescribed sinusoid around the y axis. It does this, but if I ask for a 10 degree amplitude, an amplitude of 20.102 or so results, slightly more than twice what Function::Sinusoid returns.

Am I misunderstanding something about how these constraints work? Thanks for any help. -- Andy

#include "Simbody.h"
#include <fstream>
#include <iostream>

using namespace SimTK;
using namespace std;

class PositionReporter : public PeriodicEventReporter {
public:
PositionReporter(const MultibodySystem& system, const MobilizedBody& mobod1, Real reportInterval)
: PeriodicEventReporter(reportInterval), system(system), mobod1(mobod1){}

void handleEvent(const State& state) const {
Vec3 pos1 = mobod1.getBodyOriginLocation(state);
Vec3 angvel1 = mobod1.getBodyAngularVelocity(state);
Rotation R_GB1 = mobod1.getBodyRotation(state);
Vec3 _321Angles1 = R_GB1.convertThreeAxesRotationToThreeAngles(BodyRotationSequence,ZAxis,YAxis,XAxis);

std::cout << state.getTime() << "\t" << _321Angles1[1]*180/M_PI << "\t" << angvel1[1]*180/M_PI << "\t" << std::endl;
}
private:
const MultibodySystem& system;
const MobilizedBody& mobod1;
};

int main() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);

Body::Rigid Block(MassProperties(1.0,Vec3(0,0,0),Inertia(1,1,1)));
MobilizedBody::Free block(matter.Ground(),Transform(Vec3(0,0,0)), Block, Transform(Vec3(0,0,0)));

Constraint::PrescribedMotion(matter,new Function::Constant(0),block.getMobilizedBodyIndex(),MobilizerQIndex(1));
Constraint::PrescribedMotion(matter,new Function::Sinusoid(10*M_PI/180,2*M_PI/10,0),block.getMobilizedBodyIndex(),MobilizerQIndex(2));
Constraint::PrescribedMotion(matter,new Function::Constant(0),block.getMobilizedBodyIndex(),MobilizerQIndex(3));
Constraint::PrescribedMotion(matter,new Function::Constant(0),block.getMobilizedBodyIndex(),MobilizerQIndex(4));
Constraint::PrescribedMotion(matter,new Function::Constant(0),block.getMobilizedBodyIndex(),MobilizerQIndex(5));
Constraint::PrescribedMotion(matter,new Function::Constant(0),block.getMobilizedBodyIndex(),MobilizerQIndex(6));

Visualizer viz(system);

system.addEventReporter(new Visualizer::Reporter(viz, 0.05));
system.addEventReporter(new PositionReporter(system,block,.1));

system.realizeTopology();

State state = system.getDefaultState();

RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-8);
TimeStepper ts(system, integ);
ts.initialize(state);

ts.stepTo(20.0);
}

[Michael Sherman]

Hi, Andrew.

Simbody's Free mobilizer uses a quaternion rather than angles for orientation (documentation)

Quaternions use a half-angle representation which likely explains the nearly-off-by-2 you're seeing. However, there are other differences (not the least of which is that there are four elements rather than three!) and you don't want to be prescribing quaternions directly.

The easiest fix is to make use of a mobilizer that uses angles for orientation rather than a quaternion. Consider using the Bushing mobilizer instead of Free. Note that there is a singularity when the second angle nears 90 degrees -- be careful to avoid that configuration!

Regards,
Sherm

[Jason Everett]

Hi Sherm,

Is there a reason for not prescribing quaternions, as you warn against here? Just for simplicity's sake and to avoid risky quaternion derivatives?

Thanks,
Jason

[Michael Sherman]

I don't think there is any theoretical reason you couldn't prescribe quaternions but there are practical issues having to do with the fact that the generalized velocities are not 4 quaternion derivatives but rather 3 angular velocities. I'm not certain that Simbody's API would allow prescribing motion that way and it definitely hasn't been tested! If you manage to get it working correctly, please post.

Sherm