/** {@inheritDoc} */ @Override default void setJointPosition(Tuple3DReadOnly jointTranslation) { getJointPose().setPosition(jointTranslation); }
/** {@inheritDoc} */ @Override default void setJointOrientation(Orientation3DReadOnly jointOrientation) { getJointPose().setOrientation(jointOrientation); }
/** {@inheritDoc} */ @Override default void setJointConfigurationToZero() { getJointPose().setToZero(); }
/** * Integrates the given {@code joint}'s velocity to update its configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void integrateFromVelocity(FloatingJointBasics joint) { integrate(joint.getJointTwist(), joint.getJointPose()); }
private RobotConfigurationData extractRobotConfigurationData(Pair<FloatingJointBasics, OneDoFJointBasics[]> initialFullRobotModel) { OneDoFJointBasics[] joints = initialFullRobotModel.getRight(); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(joints, new ForceSensorDefinition[0], new IMUDefinition[0]); RobotConfigurationDataFactory.packJointState(robotConfigurationData, Arrays.stream(joints).collect(Collectors.toList())); FloatingJointBasics rootJoint = initialFullRobotModel.getLeft(); if (rootJoint != null) { robotConfigurationData.getRootTranslation().set(rootJoint.getJointPose().getPosition()); robotConfigurationData.getRootOrientation().set(rootJoint.getJointPose().getOrientation()); } return robotConfigurationData; } }
/** * 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(FloatingJointBasics joint) { doubleIntegrate(joint.getJointAcceleration(), joint.getJointTwist(), joint.getJointPose()); }
public void recordCurrentState() { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); rootJointTranslation.set(rootJoint.getJointPose().getPosition()); rootJointRotation.set(rootJoint.getJointPose().getOrientation()); yoRootJointTranslation.set(rootJointTranslation); yoRootJointRotation.set(rootJointRotation); }
public static RobotConfigurationData extractRobotConfigurationData(FullHumanoidRobotModel fullRobotModel) { OneDoFJointBasics[] joints = FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(joints, new ForceSensorDefinition[0], new IMUDefinition[0]); RobotConfigurationDataFactory.packJointState(robotConfigurationData, Arrays.stream(joints).collect(Collectors.toList())); robotConfigurationData.getRootTranslation().set(fullRobotModel.getRootJoint().getJointPose().getPosition()); robotConfigurationData.getRootOrientation().set(fullRobotModel.getRootJoint().getJointPose().getOrientation()); return robotConfigurationData; }
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(); }
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); }
privilegedRootJointPosition.set(rootJoint.getJointPose().getPosition()); Quaternion privilegedRootJointOrientation = new Quaternion(); privilegedRootJointOrientation.set(rootJoint.getJointPose().getOrientation());
@Override public void update(long timestamp) { fullRobot.updateFrames(); latestTimestamp = timestamp; if(rootJoint != null) { robot.setOrientation(rootJoint.getJointPose().getOrientation()); robot.setPositionInWorld(rootJoint.getJointPose().getPosition()); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }
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()); rootJoint.getPredecessor().updateFramesRecursively();
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()); desiredRootJoint.setJointVelocity(0, new DenseMatrix64F(6, 1));
@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()); }
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)); } }
PrintTools.debug("QuadTree has changed, sending packet"); Point3D rootJointPosition = new Point3D(); rootJointPosition.set(rootJoint.getJointPose().getPosition()); robotPosition2d.set(rootJointPosition.getX(), rootJointPosition.getY()); reportMessage(HeightQuadTreeMessageConverter.convertQuadTreeForGround(quadTree, robotPosition2d, quadTreeMessageMaxRadius));
Pose3DReadOnly initialPose = new Pose3D(controllerRootJoint.getJointPose()); Pose3D aboveInitialPose = new Pose3D(initialPose); aboveInitialPose.prependTranslation(0.0, 0.0, 0.1); double desiredHeight = controllerRootJoint.getJointPose().getZ() - 0.2; drcSimulationTestHelper.publishToController(HumanoidMessageTools.createPelvisHeightTrajectoryMessage(0.5, desiredHeight, worldFrame, worldFrame));
position.set(rootJoint.getJointPose().getPosition()); linearVelocity.set(rootJoint.getJointTwist().getLinearPart());