You've already forked LuckyWorld
WIP - Json/Parquet
+ Compute stats for each episode + Skeletal for json / parquet functions
This commit is contained in:
JB Briant
@ -24,10 +24,9 @@ void URobotPilotSO100Component::BeginPlay()
|
||||
void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType,
|
||||
FActorComponentTickFunction* ThisTickFunction)
|
||||
{
|
||||
// Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
// const auto Joints = GetCurrentJointsFromPhysicsScene();
|
||||
// const auto Controls = GetCurrentControlsFromPhysicScene();
|
||||
// UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw);
|
||||
// We can buffer data in the tick because it will never be called in parallel of MujocoVolumeActor tick
|
||||
ControlsDataBuffer.Add(GetCurrentControlsFromPhysicScene());
|
||||
JointsDataBuffer.Add(GetCurrentJointsFromPhysicsScene());
|
||||
}
|
||||
|
||||
FTransform URobotPilotSO100Component::GetReachableTransform()
|
||||
@ -44,8 +43,6 @@ FTransform URobotPilotSO100Component::GetReachableTransform()
|
||||
const float RandomYaw = MaxYaw * FMath::RandRange(0., 1.) * (FMath::RandBool() ? 1 : -1);
|
||||
const FQuat RandomRotation = FRotator{0,RandomYaw,0}.Quaternion() * ArmWorldRotation;
|
||||
|
||||
|
||||
|
||||
// Compute Random Range within reach of the arm and add this to pivot location
|
||||
// Add a bit more than the Jaw Offset - TODO Offsets must be better computed
|
||||
const float RandomRange = JawOffset.X + MaxRange * FMath::RandRange(0.1f, 1.f);
|
||||
@ -91,11 +88,179 @@ void URobotPilotSO100Component::ReceiveRemoteCommand(const FRemoteControlPayload
|
||||
{
|
||||
for (const auto& [ActuatorName, ActuatorValue] : RemoteRobotPayload.Commands)
|
||||
{
|
||||
// Will print an error message if it doesn't exists
|
||||
// Will print an error message if it doesn't exist
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(ActuatorName, ActuatorValue);
|
||||
}
|
||||
}
|
||||
|
||||
FTrainingEpisodeData URobotPilotSO100Component::GetTrainingEpisodeData()
|
||||
{
|
||||
const auto TrainingData = FTrainingEpisodeData
|
||||
{
|
||||
GetBufferedJointsData(),
|
||||
GetBufferedControlsData(),
|
||||
GetStats(ControlsDataBuffer),
|
||||
GetStats(JointsDataBuffer)
|
||||
};
|
||||
|
||||
ControlsDataBuffer.Empty();
|
||||
JointsDataBuffer.Empty();
|
||||
return TrainingData;
|
||||
}
|
||||
|
||||
FJsonObject URobotPilotSO100Component::GetBufferedControlsData()
|
||||
{
|
||||
auto BufferedData = FJsonObject();
|
||||
TArray<TSharedPtr<FJsonValue>> ControlsArray;
|
||||
|
||||
for (const auto& Control : ControlsDataBuffer)
|
||||
{
|
||||
TSharedPtr<FJsonObject> ControlJson = MakeShared<FJsonObject>();
|
||||
|
||||
ControlJson->SetNumberField(TEXT("Rotation"), Control.Rotation);
|
||||
ControlJson->SetNumberField(TEXT("Pitch"), Control.Pitch);
|
||||
ControlJson->SetNumberField(TEXT("Elbow"), Control.Elbow);
|
||||
ControlJson->SetNumberField(TEXT("WristPitch"), Control.WristPitch);
|
||||
ControlJson->SetNumberField(TEXT("WristRoll"), Control.WristRoll);
|
||||
ControlJson->SetNumberField(TEXT("Jaw"), Control.Jaw);
|
||||
|
||||
ControlsArray.Add(MakeShared<FJsonValueObject>(ControlJson));
|
||||
}
|
||||
|
||||
BufferedData.SetArrayField(TEXT("action"), ControlsArray);
|
||||
|
||||
return BufferedData;
|
||||
}
|
||||
|
||||
FJsonObject URobotPilotSO100Component::GetBufferedJointsData()
|
||||
{
|
||||
auto BufferedData = FJsonObject();
|
||||
TArray<TSharedPtr<FJsonValue>> ControlsArray;
|
||||
|
||||
for (const auto& Joint : JointsDataBuffer)
|
||||
{
|
||||
TSharedPtr<FJsonObject> ControlJson = MakeShared<FJsonObject>();
|
||||
|
||||
ControlJson->SetNumberField(TEXT("Rotation"), Joint.Rotation);
|
||||
ControlJson->SetNumberField(TEXT("Pitch"), Joint.Pitch);
|
||||
ControlJson->SetNumberField(TEXT("Elbow"), Joint.Elbow);
|
||||
ControlJson->SetNumberField(TEXT("WristPitch"), Joint.WristPitch);
|
||||
ControlJson->SetNumberField(TEXT("WristRoll"), Joint.WristRoll);
|
||||
ControlJson->SetNumberField(TEXT("Jaw"), Joint.Jaw);
|
||||
|
||||
ControlsArray.Add(MakeShared<FJsonValueObject>(ControlJson));
|
||||
}
|
||||
|
||||
BufferedData.SetArrayField(TEXT("observation.state"), ControlsArray);
|
||||
|
||||
return BufferedData;
|
||||
}
|
||||
|
||||
FJsonObject URobotPilotSO100Component::GetStats(const TArray<FSo100Actuators>& ActuatorStates)
|
||||
{
|
||||
FSo100Actuators Min;
|
||||
FSo100Actuators Max;
|
||||
FSo100Actuators Sum = FSo100Actuators();
|
||||
FSo100Actuators Variance = FSo100Actuators();
|
||||
FSo100Actuators Mean;
|
||||
FSo100Actuators StdDev;
|
||||
|
||||
// Compute Sum
|
||||
for (const auto& State : ActuatorStates)
|
||||
{
|
||||
Sum.Rotation += State.Rotation;
|
||||
Sum.Pitch += State.Pitch;
|
||||
Sum.Elbow += State.Elbow;
|
||||
Sum.WristPitch += State.WristPitch;
|
||||
Sum.WristRoll += State.WristRoll;
|
||||
Sum.Jaw += State.Jaw;
|
||||
|
||||
// Assign Max
|
||||
if(State.Rotation > Max.Rotation) Max.Rotation = State.Rotation;
|
||||
if(State.Pitch > Max.Pitch) Max.Pitch = State.Pitch;
|
||||
if(State.Elbow > Max.Elbow) Max.Elbow = State.Elbow;
|
||||
if(State.WristPitch > Max.WristPitch) Max.WristPitch = State.WristPitch;
|
||||
if(State.WristRoll > Max.WristRoll) Max.WristRoll = State.WristRoll;
|
||||
if(State.Jaw > Max.Jaw) Max.Jaw = State.Jaw;
|
||||
|
||||
// Assing min
|
||||
if(State.Rotation < Min.Rotation) Min.Rotation = State.Rotation;
|
||||
if(State.Pitch < Min.Pitch) Min.Pitch = State.Pitch;
|
||||
if(State.Elbow < Min.Elbow) Min.Elbow = State.Elbow;
|
||||
if(State.WristPitch < Min.WristPitch) Min.WristPitch = State.WristPitch;
|
||||
if(State.WristRoll < Min.WristRoll) Min.WristRoll = State.WristRoll;
|
||||
if(State.Jaw < Min.Jaw) Min.Jaw = State.Jaw;
|
||||
|
||||
}
|
||||
|
||||
// Compute Mean
|
||||
Mean.Rotation = Sum.Rotation / ActuatorStates.Num();
|
||||
Mean.Pitch = Sum.Pitch / ActuatorStates.Num();
|
||||
Mean.Elbow = Sum.Elbow / ActuatorStates.Num();
|
||||
Mean.WristPitch = Sum.WristPitch / ActuatorStates.Num();
|
||||
Mean.WristRoll = Sum.WristRoll / ActuatorStates.Num();
|
||||
Mean.Jaw = Sum.Jaw / ActuatorStates.Num();
|
||||
|
||||
// Compute Variance
|
||||
// Pre step
|
||||
for (const auto& State : ActuatorStates)
|
||||
{
|
||||
const double DiffRotation = State.Rotation - Mean.Rotation;
|
||||
const double DiffPitch = State.Pitch - Mean.Pitch;
|
||||
const double DiffElbow = State.Elbow - Mean.Elbow;
|
||||
const double DiffWristPitch = State.WristPitch - Mean.WristPitch;
|
||||
const double DiffWristRoll = State.WristRoll - Mean.WristRoll;
|
||||
const double DiffJaw = State.Jaw - Mean.Jaw;
|
||||
|
||||
Variance.Rotation += DiffRotation * DiffRotation;
|
||||
Variance.Pitch += DiffPitch * DiffPitch;
|
||||
Variance.Elbow += DiffElbow * DiffElbow;
|
||||
Variance.WristPitch += DiffWristPitch * DiffWristPitch;
|
||||
Variance.WristRoll += DiffWristRoll * DiffWristRoll;
|
||||
Variance.Jaw += DiffJaw * DiffJaw;
|
||||
}
|
||||
|
||||
// Final Variance Value per actuator
|
||||
Variance.Rotation /= ActuatorStates.Num();
|
||||
Variance.Pitch /= ActuatorStates.Num();
|
||||
Variance.Elbow /= ActuatorStates.Num();
|
||||
Variance.WristPitch /= ActuatorStates.Num();
|
||||
Variance.WristRoll /= ActuatorStates.Num();
|
||||
Variance.Jaw /= ActuatorStates.Num();
|
||||
|
||||
// Standard Deviation
|
||||
StdDev.Rotation = FMath::Sqrt(Variance.Rotation);
|
||||
StdDev.Pitch = FMath::Sqrt(Variance.Pitch);
|
||||
StdDev.Elbow = FMath::Sqrt(Variance.Elbow);
|
||||
StdDev.WristPitch = FMath::Sqrt(Variance.WristPitch);
|
||||
StdDev.WristRoll = FMath::Sqrt(Variance.WristRoll);
|
||||
StdDev.Jaw = FMath::Sqrt(Variance.Jaw);
|
||||
|
||||
// Here create a json in the following format
|
||||
FJsonObject StatsJson = FJsonObject();
|
||||
|
||||
// Helper lambda to convert FSo100Actuators to TArray<TSharedPtr<FJsonValue>>
|
||||
auto ActuatorsToJsonArray = [](const FSo100Actuators& Actuators) -> TArray<TSharedPtr<FJsonValue>>
|
||||
{
|
||||
return {
|
||||
MakeShared<FJsonValueNumber>(Actuators.Rotation),
|
||||
MakeShared<FJsonValueNumber>(Actuators.Pitch),
|
||||
MakeShared<FJsonValueNumber>(Actuators.Elbow),
|
||||
MakeShared<FJsonValueNumber>(Actuators.WristPitch),
|
||||
MakeShared<FJsonValueNumber>(Actuators.WristRoll),
|
||||
MakeShared<FJsonValueNumber>(Actuators.Jaw)
|
||||
};
|
||||
};
|
||||
|
||||
StatsJson.SetArrayField(TEXT("min"), ActuatorsToJsonArray(Min));
|
||||
StatsJson.SetArrayField(TEXT("max"), ActuatorsToJsonArray(Max));
|
||||
StatsJson.SetArrayField(TEXT("mean"), ActuatorsToJsonArray(Mean));
|
||||
StatsJson.SetArrayField(TEXT("std"), ActuatorsToJsonArray(StdDev));
|
||||
StatsJson.SetArrayField(TEXT("count"), { MakeShared<FJsonValueNumber>(ActuatorStates.Num()) });
|
||||
|
||||
return StatsJson;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::PrintCurrentActuators() const
|
||||
{
|
||||
PrintActuators(GetCurrentControlsFromPhysicScene());
|
||||
|
||||
Reference in New Issue
Block a user