38 unsigned int segNr = (
unsigned int)segmentNr;
40 segNr=chain.getNrOfSegments();
44 if(q_in.
rows()!=chain.getNrOfJoints())
46 else if(segNr>chain.getNrOfSegments())
50 for(
unsigned int i=0;i<segNr;i++){
51 p_out = p_out*chain.getSegment(i).pose(((
JntArray&)q_in)(j));
52 j+=chain.getSegment(i).getJoint().getNDof();