public static RobotConfigurationData nextRobotConfigurationData(Random random) { RobotConfigurationData next = new RobotConfigurationData(); int size = random.nextInt(10000); next.setTimestamp(random.nextLong()); next.setSensorHeadPpsTimestamp(random.nextLong()); next.setJointNameHash(random.nextInt(10000)); next.getJointAngles().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getJointVelocities().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getJointTorques().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getRootTranslation().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getPelvisLinearVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getPelvisAngularVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getRootOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random)); next.getPelvisLinearAcceleration().set(EuclidCoreRandomTools.nextVector3D32(random)); size = Math.abs(random.nextInt(1000)); for (int i = 0; i < next.getForceSensorData().size(); i++) next.getForceSensorData().add().set(nextSpatialVectorMessage(random)); for (IMUPacket imuPacket : nextIMUPackets(random)) next.getImuSensorData().add().set(imuPacket); next.setRobotMotionStatus(RandomNumbers.nextEnum(random, RobotMotionStatus.class).toByte()); next.setLastReceivedPacketTypeId(random.nextInt(1000)); next.setLastReceivedPacketUniqueId(random.nextLong()); next.setLastReceivedPacketRobotTimestamp(random.nextLong()); return next; }
robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME); robotMotionStatusPublisher.publish(RobotMotionStatus.fromByte(robotConfigurationData.getRobotMotionStatus()).name()); robotBehaviorPublisher.publish(RobotMotionStatus.fromByte(robotConfigurationData.getRobotMotionStatus()).getBehaviorId());
robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME); robotMotionStatusPublisher.publish(robotConfigurationData.getRobotMotionStatus().name()); robotBehaviorPublisher.publish(robotConfigurationData.getRobotMotionStatus().getBehaviorId());
robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME); robotMotionStatusPublisher.publish(robotConfigurationData.getRobotMotionStatus().name()); robotBehaviorPublisher.publish(robotConfigurationData.getRobotMotionStatus().getBehaviorId());
public void sendMockRobotConfiguration(long totalNsecs) { IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel), forceSensorDefinitions, imuDefinitions); for (int sensorNumber = 0; sensorNumber < imuDefinitions.length; sensorNumber++) { robotConfigurationData.getImuSensorData().add(); } robotConfigurationData.setRobotMotionStatus(RobotMotionStatus.STANDING.toByte()); robotConfigurationData.setTimestamp(totalNsecs); Vector3D translation = new Vector3D(); Quaternion orientation = new Quaternion(); robotConfigurationData.getRootTranslation().set(translation); robotConfigurationData.getRootOrientation().set(orientation); publisher.publish(robotConfigurationData); }
public void receivedClockMessage(long totalNsecs) { RigidBodyTransform pelvisPoseInMocapFrame = atomicPelvisPose.get(); IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(fullRobotModel.getOneDoFJoints(), forceSensorDefinitions, imuDefinitions); for(int sensorNumber = 0; sensorNumber < imuDefinitions.length; sensorNumber++) { IMUPacket imuPacket = robotConfigurationData.getImuSensorData().add(); imuPacket.getLinearAcceleration().set(RandomGeometry.nextVector3D32(random)); imuPacket.getOrientation().set(RandomGeometry.nextQuaternion32(random)); imuPacket.getAngularVelocity().set(RandomGeometry.nextVector3D32(random)); } robotConfigurationData.setRobotMotionStatus(RobotMotionStatus.STANDING.toByte()); robotConfigurationData.setTimestamp(totalNsecs); if(pelvisPoseInMocapFrame != null) { Vector3D translation = new Vector3D(); Quaternion orientation = new Quaternion(); pelvisPoseInMocapFrame.getTranslation(translation); pelvisPoseInMocapFrame.getRotation(orientation); robotConfigurationData.getRootTranslation().set(translation); robotConfigurationData.getRootOrientation().set(orientation); } fullRobotModel.updateFrames(); packetCommunicator.send(robotConfigurationData); }