FixedJoint* EditorLoader::AddJoint(SimpleObject* pObj1, SimpleObject* pObj2)
{
m_pPhysXEngine->GetScene()->resetJointIterator();
unsigned int joints = m_pPhysXEngine->GetScene()->getNbJoints();
bool bExists(false);
NxJoint* j(0);
for (unsigned int i(0); i < joints; ++i)
{
NxJoint* joint(m_pPhysXEngine->GetScene()->getNextJoint());
NxActor* pActor1, *pActor2;
joint->getActors(&pActor1,&pActor2);
if ((pObj1->GetActor() == pActor1 || pObj1->GetActor() == pActor2) && (pObj2->GetActor() == pActor1 || pObj2->GetActor() == pActor2))
{
bExists = true;
j = joint;
}
}
if (bExists && j != 0)
{
m_pPhysXEngine->GetPhysXLock().lock();
m_pPhysXEngine->GetScene()->releaseJoint(*j);
m_pPhysXEngine->GetPhysXLock().unlock();
}
else
return new FixedJoint(m_pPhysXEngine, pObj1->GetActor(), pObj2->GetActor());
return 0;
}
void DynamixelServo::angle(float newAngle) {
int rev=0;
float diffAngle=newAngle-presentAngle;
presentAngle=newAngle;
while (diffAngle>180.0) {
rev--;
diffAngle-=360.0;
}
while(diffAngle<-180.0){
rev++;
diffAngle+=360.0;
}
int pos=DXL2USB.readWord(id,DXL_PRESENT_POSITION_WORD);
while (rev>0){
wheel(1023);
int newpos=DXL2USB.readWord(id,DXL_PRESENT_POSITION_WORD);
if (pos-newpos>DynamixelInterface::JITTER) rev--;
pos=newpos;
}
while (rev<0){
wheel(1024+1023);
int newpos=DXL2USB.readWord(id,DXL_PRESENT_POSITION_WORD);
if (newpos-pos>DynamixelInterface::JITTER) rev++;
pos=newpos;
}
diffAngle=presentAngle;
while (diffAngle>180)diffAngle-=360.0;
while (diffAngle<-180) diffAngle+=360.0;
int angle=((180-diffAngle)*2047)/180;
joint(angle);
}
std::shared_ptr<Joint> Joints::create_joint(PhysicsWorld &phys_world, Body &bodyA, Body &bodyB, int type)
{
//Get the Physics Context
PhysicsContext pc = phys_world.get_pc();
switch(type)
{
default:
case 0: // Distance joint
{
DistanceJointDescription joint_desc(phys_world);
joint_desc.set_bodies(bodyA, bodyB, bodyA.get_position(), bodyB.get_position());
joint_desc.set_damping_ratio(1.0f);
joint_desc.set_length(100.0f);
std::shared_ptr<Joint> joint( static_cast<Joint *> (new DistanceJoint(pc, joint_desc)));
return joint;
}
case 1: // Revolute joint
{
RevoluteJointDescription joint_desc(phys_world);
joint_desc.set_bodies(bodyA, bodyB, bodyA.get_position());
joint_desc.set_as_motor();
joint_desc.set_motor_speed(Angle(60,angle_degrees));
joint_desc.set_max_motor_torque(1000);
std::shared_ptr<Joint> joint( static_cast<Joint *> (new RevoluteJoint(pc, joint_desc)));
return joint;
}
case 2: // Prismatic joint
{
PrismaticJointDescription joint_desc(phys_world);
joint_desc.set_bodies(bodyA, bodyB, bodyA.get_position(), bodyB.get_position());
joint_desc.set_as_motor();
joint_desc.enable_limit();
joint_desc.set_axis_a(Vec2f(0.0f,1.0f));
joint_desc.set_motor_speed(Angle(20,angle_degrees));
joint_desc.set_translation_limits(10.0f,100.0f);
joint_desc.set_max_motor_force(1000);
std::shared_ptr<Joint> joint( static_cast<Joint *> (new PrismaticJoint(pc, joint_desc)));
return joint;
}
}
}
boost::shared_ptr< Joint > DummyInterface::createJoint(const std::string& name)
{
Joint::Ptr joint(new DummyJoint(name));
joint->name = name;
joint->feedback.pos = 0;
return joint;
}
void PyBody::setPosture(PyObject *v)
{
if (PySequence_Size(v) != numJoints()) return;
for (unsigned int i=0; i<numJoints(); i++) {
hrp::Link *j = joint(i);
if (j) j->q = boost::python::extract<double>(PySequence_GetItem(v, i));
}
notifyChanged(KINEMATICS);
}
Node* NavMesh::EulerianNext(Node *start)
{
AdjacencyList::Iterator arc;
for( arc = start->adjacents.begin(); arc != start->adjacents.end(); ++arc )
{
if( !joint(*arc) )
return (*arc)->to;
}
}
PyObject *PyBody::getPosture()
{
boost::python::list retval;
for (unsigned int i=0; i<numJoints(); i++){
hrp::Link *j = joint(i);
double q = j ? j->q : 0;
retval.append(boost::python::object(q));
}
return boost::python::incref(retval.ptr());
}
int countDegreesOfFreedom(taoDNode * node)
{
int dof(0);
for (taoJoint * joint(node->getJointList()); 0 != joint; joint = joint->getNext()) {
dof += joint->getDOF();
}
for (taoDNode * child(node->getDChild()); 0 != child; child = child->getDSibling()) {
dof += countDegreesOfFreedom(child);
}
return dof;
}
int countNumberOfJoints(taoDNode * node)
{
int count(0);
for (taoJoint * joint(node->getJointList()); 0 != joint; joint = joint->getNext()) {
++count;
}
for (taoDNode * child(node->getDChild()); 0 != child; child = child->getDSibling()) {
count += countNumberOfJoints(child);
}
return count;
}
void Model::
setState(State const & state)
{
state_ = state;
for (size_t ii(0); ii < ndof_; ++ii) {
taoJoint * joint(kgm_tree_->info[ii].joint);
joint->setQ(&const_cast<State&>(state).position_.coeffRef(ii));
joint->zeroDQ();
joint->zeroDDQ();
joint->zeroTau();
}
if (cc_tree_) {
for (size_t ii(0); ii < ndof_; ++ii) {
taoJoint * joint(cc_tree_->info[ii].joint);
joint->setQ(&const_cast<State&>(state).position_.coeffRef(ii));
joint->setDQ(&const_cast<State&>(state).velocity_.coeffRef(ii));
joint->zeroDDQ();
joint->zeroTau();
}
}
}
void fist(float wristangle){
matrixPush(ModelView);
matrixRotate(ModelView, wristangle, 0, 0, 1);
matrixPush(ModelView);
matrixScale(ModelView, .5, .5, 1);
joint();
matrixPop(ModelView);
hand();
matrixPop(ModelView);
}
MayaTransformWriter::MayaTransformWriter(double iFrame,
MayaTransformWriter & iParent, MDagPath & iDag,
uint32_t iTimeIndex,
bool iWriteVisibility)
{
if (iDag.hasFn(MFn::kJoint))
{
MFnIkJoint joint(iDag);
Alembic::AbcGeom::OXform obj(iParent.getObject(), joint.name().asChar(),
iTimeIndex);
mSchema = obj.getSchema();
Alembic::Abc::OCompoundProperty cp = obj.getProperties();
mAttrs = AttributesWriterPtr(new AttributesWriter(iFrame, cp, joint,
iTimeIndex, iWriteVisibility));
pushTransformStack(iFrame, joint);
}
else
{
MFnTransform trans(iDag);
Alembic::AbcGeom::OXform obj(iParent.getObject(), trans.name().asChar(),
iTimeIndex);
mSchema = obj.getSchema();
Alembic::Abc::OCompoundProperty cp = obj.getProperties();
mAttrs = AttributesWriterPtr(new AttributesWriter(iFrame, cp, trans,
iTimeIndex, iWriteVisibility));
pushTransformStack(iFrame, trans);
}
// need to look at inheritsTransform
MFnDagNode dagNode(iDag);
MPlug inheritPlug = dagNode.findPlug("inheritsTransform");
if (!inheritPlug.isNull())
{
if (util::getSampledType(inheritPlug) != 0)
mInheritsPlug = inheritPlug;
mSample.setInheritsXforms(inheritPlug.asBool());
}
// everything is default, don't write anything
if (mSample.getNumOps() == 0 && mSample.getInheritsXforms())
return;
mSchema.set(mSample);
}
int main(int argc, char *argv[])
{
reader pd("../config/config.ini");
camera cam;
cam.fx = atof(pd.get("camera.fx").c_str());
cam.fy = atof(pd.get("camera.fy").c_str());
cam.cx = atof(pd.get("camera.cx").c_str());
cam.cy = atof(pd.get("camera.cy").c_str());
cam.scale = atof(pd.get("camera.scale").c_str());
frame frame1, frame2;
frame1.rgb = imread("../data/rgb1.png");
frame1.depth = imread("../data/depth1.png", -1);
frame2.rgb = imread("../data/rgb2.png");
frame2.depth = imread("../data/depth2.png", -1);
cout << "extracting features" << endl;
string detector = pd.get("detector");
string descriptor = pd.get("descriptor");
compute_feature(frame1, detector, descriptor);
compute_feature(frame2, detector, descriptor);
cout << "matching" << endl;
vector<DMatch> matches;
match(matches, frame1, frame2);
cout << "solving pnp" << endl;
pnp result = sfm(cam, matches, frame1, frame2);
Isometry3d T = cvmat2eigen(result.r, result.t);
cout << "r: " << result.r << endl << "t: " << result.t << endl;
cout << "converting images into clouds" << endl;
Pointcloud::Ptr cloud1 = map2point(cam, frame1.rgb, frame1.depth);
cout << "combining clouds" << endl;
Pointcloud::Ptr output = joint(cam, cloud1, T, frame2);
io::savePCDFile("../data/result.pcd", *output);
cout << "result saved." << endl;
visualization::CloudViewer viewer("viewer");
viewer.showCloud(output);
while(!viewer.wasStopped()) {}
return 0;
}
// In order to correctly extract skinweights we first
// need to go into bindpose. (and we need to remember the current
// pose, to be able to undo it).
//
// I know: nearly no error-checking.
void
BindPoseTool::GoIntoBindPose()
{
MStatus status;
std::cout << " Going into bindpose ";
// == turn IK off
// save current state
undoInfo.ikSnap = MIkSystem::isGlobalSnap(&status);
undoInfo.ikSolve = MIkSystem::isGlobalSolve();
// turn it off
MIkSystem::setGlobalSnap(false);
MIkSystem::setGlobalSolve(false);
// == put joints into bindPose
for (MItDag dagIt(MItDag::kDepthFirst, MFn::kJoint);
!dagIt.isDone();
dagIt.next())
{
std::cout << ".";
MDagPath jointPath;
dagIt.getPath(jointPath);
if (jointPath.isInstanced())
{ // we only work on the first instance of an instanced joint.
if (jointPath.instanceNumber() != 0)
continue;
}
BindPoseUndoInformation::JointMatrixVector::value_type joint2transform;
MFnIkJoint joint(jointPath.node());
MTransformationMatrix currentTransform = joint.transformation(&status);
joint2transform.first = jointPath.node();
joint2transform.second = currentTransform;
undoInfo.savedTransforms.push_back(joint2transform);
MMatrix bindPoseMatrix = getBindPoseMatrix(joint);
joint.set(bindPoseMatrix);
}
std::cout << std::endl;
//cout << "bindPose end" << endl;
// don't know, if this is really necessary, but MS xporttranslator
// does it...
syncMeshes();
// remember the change
inBindPose = true;
}
void Model::
setState(State const & state)
{
state_ = state;
double const * pos(&state.position_[0]);
for (size_t ii(0); ii < ndof_; ++ii, ++pos) {
taoJoint * joint(kgm_joints_[ii]);
joint->setQ(pos);
joint->zeroDQ();
joint->zeroDDQ();
joint->zeroTau();
}
if (cc_root_) {
pos = &state.position_[0];
double const * vel(&state.velocity_[0]);
for (size_t ii(0); ii < ndof_; ++ii, ++pos, ++vel) {
taoJoint * joint(cc_joints_[ii]);
joint->setQ(pos);
joint->setDQ(vel);
joint->zeroDDQ();
joint->zeroTau();
}
}
}
typename std::enable_if<is_dataset<Dataset>::value, real_type>::type
accuracy(const dataset_type& ds) const {
argument_type label = label_var();
real_type result(0);
real_type weight(0);
assignment_type a;
F posterior;
for (const auto& p : ds.assignments(arguments())) {
joint(p.first, posterior);
posterior.maximum(a);
result += p.second * (a.at(label) == p.first[label]);
weight += p.second;
}
return result / weight;
}
void feet(float footangle){
matrixPush(ModelView);
matrixRotate(ModelView, footangle, 0, 0, 1);
matrixPush(ModelView);
matrixScale(ModelView, .9, .9, 1);
joint();
matrixPop(ModelView);
matrixPush(ModelView);
matrixScale(ModelView, -1, 1, 1);
foot();
matrixPop(ModelView);
matrixPop(ModelView);
}
void leg(float upperlegangle, float lowerlegangle, float footangle){
matrixPush(ModelView);
matrixRotate(ModelView, upperlegangle, 0, 0, 1);
joint();
matrixPush(ModelView);
matrixScale(ModelView, 2, 1, 1);
arm_segment();
matrixPop(ModelView);
matrixTranslate(ModelView, 0, -6, 0);
lower_leg(lowerlegangle, footangle);
matrixPop(ModelView);
}
/*
* converts cartesian via points to joint space via points
*/
vector<ColumnVector> TrajectoryController::getJSPoints(
vector<ColumnVector>& cartesianList) {
vector<ColumnVector> jsPoints;
ColumnVector cartesian(6), currentQ(6), previousQ(6), joint(6);
Generator* gen = getGenerator(cartesianList);
if (gen != NULL) {
currentQ = 0;
joint = 0;
// - convert ith cartesian to thetas
// - if conditions satisfied, add point in path.
for (int i = 0; i <= PNT_SAMPLES; i++) {
double k = ((i * 1.0) / PNT_SAMPLES);
// cout << k << endl;
//get xyz and orientation from generator for time k.
cartesian = gen->getPosition(k);
// cout << "cart c " << cartesian.as_row();
Quaternion quat = gen->getOrientation(k);
cartesian.resize_keep(6);
Matrix R = quat.R();
cartesian.Rows(4, 6) = irpy(R);
// cout << cartesian.AsRow() << endl;
// cout << quat << endl;
//add the joint solution if it exists
if (kineSolver->getBestSolution(cartesian, currentQ, joint)) {
// cout << "cartesian " << cartesian.AsRow() << endl;
// cout << "joint " << joint.AsRow() << endl;
jsPoints.push_back(joint);
currentQ = joint;
} else {
//path has to be rejected
cout << cartesian.AsRow()
<< " : Via point out of workspace. Path rejected."
<< endl;
jsPoints.clear();
break;
}
}
}
return jsPoints;
}
void lower_leg(float lowerlegangle, float footangle){
matrixPush(ModelView);
matrixRotate(ModelView, lowerlegangle, 0, 0, 1);
matrixPush(ModelView);
matrixScale(ModelView, .9, .9, 1);
joint();
matrixPop(ModelView);
matrixPush(ModelView);
matrixScale(ModelView, 1.8, .8 , 1);
arm_segment();
matrixPop(ModelView);
matrixTranslate(ModelView, 0, -4.8, 0);
feet(footangle);
matrixPop(ModelView);
}
void JoinSegments(T* mSys, ChSharedBodyPtr& A, ChSharedBodyPtr& B) {
real mass = 1;
Quaternion q;
q.Q_from_AngZ(PI / 2.0);
ChCoordsys<> pos1;
ChCoordsys<> pos2;
pos1.pos = Vector(segment_length - segment_thickness, 0, 0);
pos2.pos = Vector(-segment_length + segment_thickness, 0, 0);
ChCoordsys<> pos;
pos.pos = Vector(0, 0, 0);
ChSharedPtr<ChLinkLockRevolute> joint(new ChLinkLockRevolute);
joint->Initialize(A, B, true, pos1, pos2);
mSys->AddLink(joint);
}
void arm(float upperarmangle, float lowerarmangle, float wristangle){
matrixPush(ModelView);
matrixRotate(ModelView, upperarmangle, 0, 0, 1);
matrixPush(ModelView);
matrixScale(ModelView, .625, .625, 1);
joint();
matrixPop(ModelView);
matrixPush(ModelView);
matrixScale(ModelView, 1.25, 1, 1);
arm_segment();
matrixPop(ModelView);
matrixTranslate(ModelView, 0, -6, 0);
forearm(lowerarmangle, wristangle);
matrixPop(ModelView);
}
void forearm(float lowerarmangle, float wristangle){
matrixPush(ModelView);
matrixRotate(ModelView, lowerarmangle, 0, 0, 1);
matrixPush(ModelView);
matrixScale(ModelView, .5, .5, 1);
joint();
matrixPop(ModelView);
arm_segment();
matrixPush(ModelView);
matrixTranslate(ModelView, 0, -6, 0);
fist(wristangle);
matrixPop(ModelView);
matrixPop(ModelView);
}
void CreateFiber(T* mSys, ChVector<> position) {
real length = .05;
real thickness = .01;
ChSharedBodyPtr Fiber1 = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Fiber2;
ChSharedBodyPtr fibers[10];
fibers[0] = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
Quaternion q;
q.Q_from_AngZ(PI / 2.0);
InitObject(fibers[0], .1, Vector(0, -2, 0) + position, q, material_fiber, true, false, -1, -2);
AddCollisionGeometry(fibers[0], CYLINDER, Vector(thickness, length, length), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(fibers[0], SPHERE, Vector(thickness, length, length), Vector(0, length, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(fibers[0], SPHERE, Vector(thickness, length, length), Vector(0, -length, 0), Quaternion(1, 0, 0, 0));
FinalizeObject(fibers[0], (ChSystemParallel *) mSys);
fibers[0]->SetPos_dt(Vector(0, -1, 0));
for (int i = 1; i < 10; i++) {
fibers[i] = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(fibers[i], .1, Vector(i * (length + thickness) * 2, -2, 0) + position, q, material_fiber, true, false, -1, -2);
AddCollisionGeometry(fibers[i], CYLINDER, Vector(thickness, length, length), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(fibers[i], SPHERE, Vector(thickness, length, length), Vector(0, length, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(fibers[i], SPHERE, Vector(thickness, length, length), Vector(0, -length, 0), Quaternion(1, 0, 0, 0));
FinalizeObject(fibers[i], (ChSystemParallel *) mSys);
fibers[i]->SetPos_dt(Vector(0, -1, 0));
ChCoordsys<> pos;
pos.pos = Vector((i - 1) * (length + thickness) * 2 + length, -2, 0) + position;
ChSharedPtr<ChLinkLockRevolute> joint(new ChLinkLockRevolute);
joint->Initialize(fibers[i - 1], fibers[i], pos);
// joint->Initialize(fibers[i - 1], fibers[i], true, Vector(length,0,0),Vector(-length,0,0),true);
// joint->Set_SpringF(100);
// joint->Set_SpringK(100);
// joint->Set_SpringR(100);
mSys->AddLink(joint);
}
}
int main() {
int t, test = 0, n, x, y, i;
scanf("%d", &t);
while(t--) {
scanf("%d", &n);
memset(map, 0, sizeof(map));
memset(deg, 0, sizeof(deg));
init();
while(n--) {
scanf("%d %d", &x, &y);
map[x][y]++;
map[y][x]++;
deg[x]++, deg[y]++;
joint(x, y);
}
printf("Case #%d\n", ++test);
int st, flag = 0;
for(i = 1; i <= 50; i++) {
if(deg[i]) {
st = i;
if(deg[i]&1)
flag = 1;
}
}
for(i = 1; i <= 50; i++) {
if(deg[i]) {
if(find(st) != find(i))
flag = 1;
}
}
if(flag)
puts("some beads may be lost");
else
dfs(st);
if(t)
puts("");
}
return 0;
}
void OpenNIDevice::copySkeleton(const nite::Skeleton& srcSkeleton, dai::Skeleton& dstSkeleton)
{
for (int j=0; j<15; ++j)
{
// Load nite::SkeletonJoint
const nite::SkeletonJoint& niteJoint = srcSkeleton.getJoint((nite::JointType) j);
const nite::Point3f& nitePos = niteJoint.getPosition();
const nite::Quaternion& niteOrientation = niteJoint.getOrientation();
// Copy nite joint pos to my own Joint converting from nite milimeters to meters
SkeletonJoint joint(Point3f(nitePos.x, nitePos.y, nitePos.z), _staticMap[j]);
joint.setOrientation(Quaternion(niteOrientation.w, niteOrientation.x,
niteOrientation.y,
niteOrientation.z));
joint.setPositionConfidence(niteJoint.getPositionConfidence());
joint.setOrientationConfidence(niteJoint.getOrientationConfidence());
dstSkeleton.setJoint(_staticMap[j], joint);
}
dstSkeleton.setDistanceUnits(dai::DISTANCE_MILIMETERS);
}
/*! @brief Copys the joint sensor data from the Motors class to the NUSensorsData container.
The Motors class is very old and uses two globals; JointPositions and JointLoads to store sensor data.
Consequently, we need only copy from these old arrays into the new fancy NUSensorsData structure.
*/
void BearSensors::copyFromJoints()
{
static const float NaN = std::numeric_limits<float>::quiet_NaN();
std::vector<float> targets;
m_motors->getTargets(targets);
std::vector<float> joint(NUSensorsData::NumJointSensorIndices, NaN);
float delta_t = (m_current_time - m_previous_time)/1000.0;
for (size_t i=0; i<m_joint_ids.size(); i++)
{
joint[NUSensorsData::PositionId] = Motors::MotorSigns[i]*(JointPositions[i] - Motors::DefaultPositions[i])/195.379; // I know, its a horrible way of converting from motor units to radians
joint[NUSensorsData::VelocityId] = (joint[NUSensorsData::PositionId] - m_previous_positions[i])/delta_t;
joint[NUSensorsData::AccelerationId] = (joint[NUSensorsData::VelocityId] - m_previous_velocities[i])/delta_t;
joint[NUSensorsData::TargetId] = Motors::MotorSigns[i]*(targets[i] - Motors::DefaultPositions[i])/195.379;;
joint[NUSensorsData::StiffnessId] = NaN;
joint[NUSensorsData::TorqueId] = Motors::MotorSigns[i]*JointLoads[i]*1.6432e-3; // This torque conversion factor was measured for a DX-117, I don't know how well it applies to other motors
m_data->set(*m_joint_ids[i], m_current_time, joint);
m_previous_positions[i] = joint[NUSensorsData::PositionId];
m_previous_velocities[i] = joint[NUSensorsData::VelocityId];
}
}
//destructor
MyoThread::~MyoThread()
{
if(mLoop) joint();
}
MayaTransformWriter::MayaTransformWriter(MayaTransformWriter & iParent,
MDagPath & iDag, Alembic::Util::uint32_t iTimeIndex, const JobArgs & iArgs)
{
mVerbose = iArgs.verbose;
mFilterEulerRotations = iArgs.filterEulerRotations;
mJointOrientOpIndex[0] = mJointOrientOpIndex[1] = mJointOrientOpIndex[2] =
mRotateOpIndex[0] = mRotateOpIndex[1] = mRotateOpIndex[2] =
mRotateAxisOpIndex[0] = mRotateAxisOpIndex[1] = mRotateAxisOpIndex[2] = ~size_t(0);
if (iDag.hasFn(MFn::kJoint))
{
MFnIkJoint joint(iDag);
MString jointName = joint.name();
mName = util::stripNamespaces(jointName, iArgs.stripNamespace);
Alembic::AbcGeom::OXform obj(iParent.getObject(), mName.asChar(),
iTimeIndex);
mSchema = obj.getSchema();
Alembic::Abc::OCompoundProperty cp;
Alembic::Abc::OCompoundProperty up;
if (AttributesWriter::hasAnyAttr(joint, iArgs))
{
cp = mSchema.getArbGeomParams();
up = mSchema.getUserProperties();
}
mAttrs = AttributesWriterPtr(new AttributesWriter(cp, up, obj, joint,
iTimeIndex, iArgs));
pushTransformStack(joint, iTimeIndex == 0);
}
else
{
MFnTransform trans(iDag);
MString transName = trans.name();
mName = util::stripNamespaces(transName, iArgs.stripNamespace);
Alembic::AbcGeom::OXform obj(iParent.getObject(), mName.asChar(),
iTimeIndex);
mSchema = obj.getSchema();
Alembic::Abc::OCompoundProperty cp;
Alembic::Abc::OCompoundProperty up;
if (AttributesWriter::hasAnyAttr(trans, iArgs))
{
cp = mSchema.getArbGeomParams();
up = mSchema.getUserProperties();
}
mAttrs = AttributesWriterPtr(new AttributesWriter(cp, up, obj, trans,
iTimeIndex, iArgs));
pushTransformStack(trans, iTimeIndex == 0);
}
// need to look at inheritsTransform
MFnDagNode dagNode(iDag);
MPlug inheritPlug = dagNode.findPlug("inheritsTransform");
if (!inheritPlug.isNull())
{
if (util::getSampledType(inheritPlug) != 0)
mInheritsPlug = inheritPlug;
mSample.setInheritsXforms(inheritPlug.asBool());
}
// everything is default, don't write anything
if (mSample.getNumOps() == 0 && mSample.getInheritsXforms())
return;
mSchema.set(mSample);
}
//----------------------------------------------------------------------------
int main( void ) {
// uncomment to log dynamics
Moby::Log<Moby::OutputToFile>::reporting_level = 7;
boost::shared_ptr<Moby::TimeSteppingSimulator> sim( new Moby::TimeSteppingSimulator() );
boost::shared_ptr<Moby::GravityForce> g( new Moby::GravityForce() );
g->gravity = Ravelin::Vector3d( 0, 0, -9.8 );
Moby::RCArticulatedBodyPtr ab( new Moby::RCArticulatedBody() );
ab->id = "gripper";
ab->algorithm_type = Moby::RCArticulatedBody::eCRB;
std::vector< Moby::RigidBodyPtr > links;
Moby::RigidBodyPtr base( new Moby::RigidBody() );
{
Moby::PrimitivePtr box( new Moby::BoxPrimitive(0.05,1.0,0.05) );
box->set_mass( 1 );
// static
base->id = "base";
base->set_visualization_data( box->create_visualization() );
base->set_inertia( box->get_inertia() );
base->set_enabled( false );
base->set_pose( Ravelin::Pose3d( Ravelin::Quatd::normalize(Ravelin::Quatd(0,0,0,1)), Ravelin::Origin3d(0,0,0) ) );
links.push_back( base );
}
Moby::RigidBodyPtr link( new Moby::RigidBody() );
{
Moby::PrimitivePtr box( new Moby::BoxPrimitive(0.05,0.05,0.5) );
box->set_mass( 1 );
link->id = "link";
link->set_visualization_data( box->create_visualization() );
link->set_inertia( box->get_inertia() );
link->set_enabled( true );
link->get_recurrent_forces().push_back( g );
link->set_pose( Ravelin::Pose3d( Ravelin::Quatd::normalize(Ravelin::Quatd(0,0,0,1)), Ravelin::Origin3d(0,0,-0.275) ) );
links.push_back( link );
}
std::vector< Moby::JointPtr > joints;
boost::shared_ptr<Moby::PrismaticJoint> joint( new Moby::PrismaticJoint() );
{
joint->id = "joint";
joint->set_location( Ravelin::Vector3d(0,0,0,base->get_pose()), base, link );
joint->set_axis( Ravelin::Vector3d(0,1,0,Moby::GLOBAL) );
joint->lolimit = -0.5;
joint->hilimit = 0.5;
joints.push_back( joint );
}
ab->set_links_and_joints( links, joints );
ab->get_recurrent_forces().push_back( g );
ab->set_floating_base(false);
ab->controller = &push_controller;
sim->add_dynamic_body( ab );
Viewer viewer( sim, Ravelin::Origin3d(-5,0,-1), Ravelin::Origin3d(0,0,-1), Ravelin::Origin3d(0,0,1) );
while(true) {
if( !viewer.update() ) break;
sim->step( 0.001 );
Ravelin::Pose3d pose = *link->get_pose();
pose.update_relative_pose(Moby::GLOBAL);
std::cout << "t: " << sim->current_time << " x: " << pose.x << std::endl;
}
return 0;
}