void JointsTrajectory::getJointsAtTimeStep(size_t time_step, samples::Joints& joints) { if(time_step > getTimeSteps()) throw(InvalidTimeStep(time_step)); joints.resize(getNumberOfJoints()); joints.names = names; for(size_t i=0; i<getNumberOfJoints(); i++){ joints.elements[i] = elements[i][time_step]; } }
void URDFRobot::updateJoints(std::vector<double> &q) { osg::Matrix m; for (int i=0; i<getNumberOfJoints(); i++) { if(jointType[i]==1){ if(mimic[i].sliderCrank==0) m.makeRotate(q[mimic[i].joint]*mimic[i].multiplier+mimic[i].offset,joint_axis[i]); else m.makeRotate((q[mimic[i].joint]+asin(mimic[i].offset*sin(q[mimic[i].joint])))*-1,joint_axis[i]); } else if(jointType[i]==2) m.makeTranslate( joint_axis[i] *= (q[mimic[i].joint]*mimic[i].multiplier+mimic[i].offset) ); else m.makeIdentity(); osg::Matrix nm=zerojoints[i]->getMatrix(); nm.preMult(m); joints[i]->setMatrix(nm); } }