/** {@inheritDoc} */ @Override public void update() { centerOfMassJacobian.reset(); super.update(); }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { // revoluteJoint.setQ(revoluteJoint.getQ() + RandomTools.generateRandomDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.setQ(RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }
/** * Request all the children joints of this rigid-body to update their reference frame and request * their own successor to call this same method. * <p> * By calling this method on the root body of a robot, it will update the reference frames of all * the robot's joints. * </p> */ default void updateFramesRecursively() { getBodyFixedFrame().update(); for (int childIndex = 0; childIndex < getChildrenJoints().size(); childIndex++) { getChildrenJoints().get(childIndex).updateFramesRecursively(); } }
@Override public void updateFrames() { fullRobotModel.updateFrames(); pelvisZUpFrame.update(); for (RobotSide robotSide : RobotSide.values) { ankleZUpFrames.get(robotSide).update(); ReferenceFrame handZUpFrame = handZUpFrames.get(robotSide); if (handZUpFrame != null) { handZUpFrame.update(); } footReferenceFrames.get(robotSide).update(); soleFrames.get(robotSide).update(); soleZUpFrames.get(robotSide).update(); } midFeetZUpFrame.update(); midFootZUpGroundFrame.update(); midFeetUnderPelvisWalkDirectionFrame.update(); centerOfMassFrame.update(); }
/** * Updates {@code afterJointFrame} of this joint to take into consideration the new joint * configuration. Then calls {@link RigidBody#updateFramesRecursively()} which in its turn updates * its {@code bodyFixedFrame} and then {@link #updateFramesRecursively()} for all of its * {@link JointBasics} child. * <p> * As a result, this method will update all the reference frames of the subtree starting from this * joint. * </p> * <p> * In addition to updating their respective poses, the reference frame also updates their velocity * based on the joint velocities. * </p> */ default void updateFramesRecursively() { getFrameBeforeJoint().update(); getFrameAfterJoint().update(); if (getSuccessor() != null) { getSuccessor().updateFramesRecursively(); } } }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { revoluteJoint.setQ(revoluteJoint.getQ() + RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJointBasics oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clamp(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
private void applyJointAngleCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJointBasics oneDoFJoint = oneDoFJoints[i]; double newQ = oneDoFJoint.getQ() - jointAnglesCorrection.get(i, 0); if (limitJointAngles) newQ = Math.min(oneDoFJoint.getJointLimitUpper(), Math.max(newQ, oneDoFJoint.getJointLimitLower())); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } if (stepListener != null) { stepListener.didAnInverseKinemticsStep(errorScalar); } }
private void updateJointAngles() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = oneDoFJoints[i].getQ() + correction.get(i, 0); newQ = MathTools.clamp(newQ, oneDoFJoints[i].getJointLimitLower(), oneDoFJoints[i].getJointLimitUpper()); oneDoFJoints[i].setQ(newQ); oneDoFJoints[i].getFrameAfterJoint().update(); } }
private void applyCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJointBasics oneDoFJoint; if (useSeed) { oneDoFJoint = oneDoFJointsSeed[i]; } else { oneDoFJoint = oneDoFJoints[i]; } double newQ = oneDoFJoint.getQ() - correction.get(i, 0); newQ = MathTools.clamp(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
private void getFloatingJointStateFromSCS() { FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0); RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D(); floatingJoint.getJointPose().set(jointTransform3D); floatingJoint.getFrameAfterJoint().update(); FrameVector3D linearVelocity = new FrameVector3D(); scsJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint()); floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity); }
randomMovingFrame.update(); zUpFrame.update();
randomMovingFrame.update(); zUpFrame.update();