/** {@inheritDoc} */ @Override default void setJointTwistToZero() { setQd(0.0); }
@Before public void setUp() { joint.setQ(0.0); joint.setQd(0.0); joint.setQdd(0.0); trajectoryTimeProvider = new ConstantDoubleProvider(timeRequired); }
/** * Sets this joint velocity from the other joint. * * @param other the other to get the velocity from. Not modified. */ default void setJointTwist(OneDoFJointReadOnly other) { setQd(other.getQd()); }
/** * Generates a random state and update the given {@code joint} with it. * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. * @param min the minimum value for the generated random value. * @param max the maximum value for the generated random value. * @param joint the joints to set the state of. Modified. */ public static void nextState(Random random, JointStateType stateToRandomize, double min, double max, OneDoFJointBasics joint) { switch (stateToRandomize) { case CONFIGURATION: joint.setQ(EuclidCoreRandomTools.nextDouble(random, min, max)); break; case VELOCITY: joint.setQd(EuclidCoreRandomTools.nextDouble(random, min, max)); break; case ACCELERATION: joint.setQdd(EuclidCoreRandomTools.nextDouble(random, min, max)); break; case EFFORT: joint.setTau(EuclidCoreRandomTools.nextDouble(random, min, max)); break; default: throw new RuntimeException("Unhandled state selection: " + stateToRandomize); } }
/** {@inheritDoc} */ @Override default int setJointVelocity(int rowStart, DenseMatrix64F matrix) { setQd(matrix.get(rowStart, 0)); return rowStart + getDegreesOfFreedom(); }
private void readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); if (pinJoint == null) continue; revoluteJoint.setQ(pinJoint.getQYoVariable().getDoubleValue()); revoluteJoint.setQd(pinJoint.getQDYoVariable().getDoubleValue()); revoluteJoint.setQdd(pinJoint.getQDDYoVariable().getDoubleValue()); revoluteJoint.setTau(pinJoint.getTau()); } }
/** * Integrates the given {@code joint}'s acceleration and velocity to update its velocity and * configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void doubleIntegrateFromAcceleration(OneDoFJointBasics joint) { double initialQ = joint.getQ(); double initialQd = joint.getQd(); double qdd = joint.getQdd(); joint.setQ(doubleIntegratePosition(qdd, initialQd, initialQ)); joint.setQd(integrateVelocity(qdd, initialQd)); }
public void updateJointVelocities_SCS_to_ID() { if (scsFloatingJoint != null) { ReferenceFrame elevatorFrame = idFloatingJoint.getFrameBeforeJoint(); ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint(); scsFloatingJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(rootBodyFrame); scsFloatingJoint.getAngularVelocity(angularVelocity, rootBodyFrame); rootJointTwist.setIncludingFrame(rootBodyFrame, elevatorFrame, rootBodyFrame, angularVelocity, linearVelocity); idFloatingJoint.setJointTwist(rootJointTwist); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue()); } }
public void setFullRobotModelStateToMatchRobot() { robot.update(); FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); setFullRobotModelRootJointPositionAndOrientationToMatchRobot(sixDoFJoint, floatingJoint); fullRobotModel.updateFrames(); setFullRobotModelRootJointVelocityAndAngularVelocityToMatchRobot(sixDoFJoint, floatingJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDoFJoint.setQ(oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDoFJoint.setQd(oneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
break; case VELOCITY: joint.setQd(EuclidCoreRandomTools.nextDouble(random, joint.getVelocityLimitLower(), joint.getVelocityLimitUpper())); break; case EFFORT:
private void getOneDoFJointStateFromSCS() { for (OneDoFJointBasics joint : allOneDoFJoints) { PinJoint scsJoint = (PinJoint) robot.getJoint(joint.getName()); joint.setQ(scsJoint.getQ()); joint.setQd(scsJoint.getQD()); double tau = scsJoint.getTau(); if (scsJoint.tauDamping != null) tau += scsJoint.tauDamping.getValue(); if (scsJoint.tauJointLimit != null) tau += scsJoint.tauJointLimit.getValue(); if (scsJoint.tauVelocityLimit != null) tau += scsJoint.tauVelocityLimit.getValue(); joint.setTau(tau); } }
@Override public void update() { joint.setQ(handle.getPosition()); joint.setQd(handle.getVelocity()); bl_qd.update(); joint.setTau(handle.getEffort()); q.set(joint.getQ()); qd.set(joint.getQd()); tau.set(joint.getTau()); positionStepSizeLimiter.updateOutput(q.getDoubleValue(), q_d.getDoubleValue()); handle.setDesiredPosition(positionStepSizeLimiter.getDoubleValue()); } }
public void setFullRobotModelStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = rootJoint.getFrameAfterJoint(); Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity), RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setJointTwist(bodyTwist); rootJoint.setJointPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll); ArrayList<OneDoFJointBasics> oneDoFJoints = new ArrayList<OneDoFJointBasics>(); fullRobotModel.getOneDoFJoints(oneDoFJoints); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { double lowerLimit = oneDoFJoint.getJointLimitLower(); double upperLimit = oneDoFJoint.getJointLimitUpper(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDoFJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDoFJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
joint.setQd(0.0);
joint.setQd(data.getDesiredVelocity());
@Override public void receivedPacket(RobotConfigurationData packet) { latestRobotConfigurationData = packet; FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); TFloatArrayList newJointAngles = packet.getJointAngles(); TFloatArrayList newJointVelocities = packet.getJointAngles(); TFloatArrayList newJointTorques = packet.getJointTorques(); OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (int i = 0; i < newJointAngles.size(); i++) { oneDoFJoints[i].setQ(newJointAngles.get(i)); oneDoFJoints[i].setQd(newJointVelocities.get(i)); oneDoFJoints[i].setTau(newJointTorques.get(i)); } pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation()); pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation()); rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ()); rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS()); computeDriftTransform(); rootJoint.getPredecessor().updateFramesRecursively(); yoVariableServer.update(System.currentTimeMillis()); }
@Override public void update() { joint.setQ(handle.getPosition()); joint.setQd(handle.getVelocity()); bl_qd.update(); joint.setTau(handle.getEffort()); q.set(joint.getQ()); qd.set(joint.getQd()); tau.set(joint.getTau()); double pdOutput = pdController.compute(q.getDoubleValue(), q_d.getDoubleValue(), qd.getDoubleValue(), qd_d.getDoubleValue()); jointCommand_pd.set(pdOutput); tau_d.set(valkyrieRosControlSliderBoard.masterScaleFactor.getDoubleValue() * (jointCommand_pd.getDoubleValue() + jointCommand_function.getDoubleValue()) + tau_offset .getDoubleValue()); handle.setDesiredEffort(tau_d.getDoubleValue()); } }
oneDoFJoints[i].setQd(0.0);
double jointVelocity = pinJoint.getQDYoVariable().getDoubleValue(); revoluteJoint.setQ(jointPosition); revoluteJoint.setQd(jointVelocity);
joint.setQd(velocityDecay * qdNew);