public void update(LongBuffer buffer) { rotation.setW(Double.longBitsToDouble(buffer.get())); rotation.setX(Double.longBitsToDouble(buffer.get())); rotation.setY(Double.longBitsToDouble(buffer.get())); rotation.setZ(Double.longBitsToDouble(buffer.get())); translation.setX(Double.longBitsToDouble(buffer.get())); translation.setY(Double.longBitsToDouble(buffer.get())); translation.setZ(Double.longBitsToDouble(buffer.get())); twist.setAngularPartX(Double.longBitsToDouble(buffer.get())); twist.setAngularPartY(Double.longBitsToDouble(buffer.get())); twist.setAngularPartZ(Double.longBitsToDouble(buffer.get())); twist.setLinearPartX(Double.longBitsToDouble(buffer.get())); twist.setLinearPartY(Double.longBitsToDouble(buffer.get())); twist.setLinearPartZ(Double.longBitsToDouble(buffer.get())); }
public void update(LongBuffer buffer) { rotation.setW(Double.longBitsToDouble(buffer.get())); rotation.setX(Double.longBitsToDouble(buffer.get())); rotation.setY(Double.longBitsToDouble(buffer.get())); rotation.setZ(Double.longBitsToDouble(buffer.get())); translation.setX(Double.longBitsToDouble(buffer.get())); translation.setY(Double.longBitsToDouble(buffer.get())); translation.setZ(Double.longBitsToDouble(buffer.get())); twist.setAngularPartX(Double.longBitsToDouble(buffer.get())); twist.setAngularPartY(Double.longBitsToDouble(buffer.get())); twist.setAngularPartZ(Double.longBitsToDouble(buffer.get())); twist.setLinearPartX(Double.longBitsToDouble(buffer.get())); twist.setLinearPartY(Double.longBitsToDouble(buffer.get())); twist.setLinearPartZ(Double.longBitsToDouble(buffer.get())); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { int index = rowStart; double qdRot = matrix.get(index++, 0); double xd = matrix.get(index++, 0); double zd = matrix.get(index++, 0); jointTwist.setToZero(); jointTwist.setAngularPartY(qdRot); jointTwist.setLinearPart(xd, 0.0, zd); }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); twistToPack.setAngularPartY(jointTwist.getAngularPartY()); twistToPack.setLinearPartX(jointTwist.getLinearPartX()); twistToPack.setLinearPartZ(jointTwist.getLinearPartZ()); }
public void setJointTwist(Twist jointTwist) { this.jointTwist.checkReferenceFramesMatch(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); this.jointTwist.setAngularPartY(jointTwist.getAngularPartY()); this.jointTwist.setLinearPartX(jointTwist.getLinearPartX()); this.jointTwist.setLinearPartZ(jointTwist.getLinearPartZ()); }