/** * Sets both position and orientation. * * @param position the tuple with the new position coordinates. Not modified. * @param orientation the new orientation. Not modified. */ default void set(Tuple3DReadOnly position, Orientation3DReadOnly orientation) { setOrientation(orientation); setPosition(position); }
/** {@inheritDoc} */ @Override default void setJointPosition(Tuple3DReadOnly jointTranslation) { getJointPose().setPosition(jointTranslation); }
/** * Sets the position from the given frame tuple 3D. * * @param position the tuple with the new position coordinates. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple3DReadOnly position) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position); }
/** * Sets the position from the given frame tuple 2D and the given {@code z} coordinate. * * @param position the tuple with the x and y position coordinates. Not modified. * @param z the new z-coordinate. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple2DReadOnly position, double z) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position, z); }
/** * Sets the position from the given frame tuple 2D. * <p> * The z component remains unchanged. * </p> * * @param position the tuple with the new position coordinates. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple2DReadOnly position) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position); }
/** * Sets all the components of this pose 3D. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @param x the x-coordinate of the position. * @param y the y-coordinate of the position. * @param z the z-coordinate of the position. * @param yaw the angle to rotate about the z-axis. * @param pitch the angle to rotate about the y-axis. * @param roll the angle to rotate about the x-axis. */ default void set(double x, double y, double z, double yaw, double pitch, double roll) { setPosition(x, y, z); setOrientationYawPitchRoll(yaw, pitch, roll); }
/** * Sets this pose 3D to the given {@code pose2DReadOnly}. * * @param pose2DReadOnly the pose 2D used to set this pose 3D. Not modified. */ default void set(Pose2DReadOnly pose2DReadOnly) { setPosition(pose2DReadOnly.getPosition(), 0.0); setOrientation(pose2DReadOnly.getOrientation()); }
/** * Sets this pose 3D to match the given rigid-body transform. * * @param rigidBodyTransform the transform use to set this pose 3D. Not modified. */ default void set(RigidBodyTransform rigidBodyTransform) { setPosition(rigidBodyTransform.getTranslationVector()); setOrientation(rigidBodyTransform.getRotationMatrix()); }
/** * Sets this pose 3D to the {@code other} pose 3D. * * @param other the other pose 3D. Not modified. */ default void set(Pose3DReadOnly other) { setPosition(other.getPosition()); setOrientation(other.getOrientation()); }
/** * Sets the position to the given tuple. * * @param position the tuple with the new position coordinates. Not modified. */ default void setPosition(Tuple3DReadOnly position) { setPosition(position.getX(), position.getY(), position.getZ()); }
/** * Sets this pose 3D to match the given quaternion-based transform. * * @param quaternionBasedTransform the transform use to set this pose 3D. Not modified. */ default void set(QuaternionBasedTransform quaternionBasedTransform) { setPosition(quaternionBasedTransform.getTranslationVector()); setOrientation(quaternionBasedTransform.getQuaternion()); }
/** * Rotates, then adds the translation (x, y, z) to this pose 3D. * <p> * Use this method if the translation (x, y, z) is expressed in the local coordinates described by * this pose 3D. Otherwise, use {@link #prependTranslation(double, double, double)}. * </p> * * @param x the translation distance along the x-axis. * @param y the translation distance along the y-axis. * @param z the translation distance along the z-axis. */ default void appendTranslation(double x, double y, double z) { double thisX = getX(); double thisY = getY(); double thisZ = getZ(); setPosition(x, y, z); getOrientation().transform(getPosition()); getPosition().add(thisX, thisY, thisZ); }
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.getJointNameHash()) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getDesiredJointAngles().get(i); OneDoFJointBasics joint = oneDoFJoints[i]; joint.setQ(q); } Vector3D translation = solution.getDesiredRootTranslation(); rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = solution.getDesiredRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); fullRobotModelToUseForConversion.updateFrames(); }
rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = robotConfigurationData.getRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());
@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()); }
desiredRootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = robotConfigurationData.getRootOrientation(); desiredRootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());