You've already forked LuckyWorld
MISC - Rename Variables
This commit is contained in:
Jb win
@ -21,7 +21,7 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T
|
||||
FActorComponentTickFunction* ThisTickFunction)
|
||||
{
|
||||
// Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
// const auto Joints = GetCurrentJointsFromPhysicScene();
|
||||
// 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);
|
||||
}
|
||||
@ -59,31 +59,46 @@ void URobotPilotSO100Component::DisableAnim()
|
||||
AnimationStartTime = 0;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::DebugUpdatePos()
|
||||
{
|
||||
const int JointId = RobotOwner->PhysicsSceneProxy->GetJointId("MujocoBody_6");
|
||||
auto MjData = RobotOwner->PhysicsSceneProxy->GetMujocoData();
|
||||
auto MjModel = RobotOwner->PhysicsSceneProxy->GetMujocoModel();
|
||||
// mj_name2id(MjModel, mjOBJ_JOINT, "");
|
||||
|
||||
if (JointId)
|
||||
{
|
||||
auto adx = MjModel.jnt_qposadr[JointId];
|
||||
mjtNum* body_pos = MjData.xpos + 3 * JointId;
|
||||
UE_LOG(LogTemp, Log, TEXT("JointId %i - pos.x %f - pos.y %f - pos.z %f"), JointId, body_pos[0], body_pos[1], body_pos[2]);
|
||||
}
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const
|
||||
{
|
||||
return FSo100Actuators
|
||||
{
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Rotation),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Pitch),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Elbow),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristPitch),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristRoll),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Jaw),
|
||||
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Rotation),
|
||||
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Pitch),
|
||||
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Elbow),
|
||||
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristPitch),
|
||||
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristRoll),
|
||||
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Jaw),
|
||||
};
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicScene() const
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicsScene() const
|
||||
{
|
||||
// Due to Mujoco import plugin not renaming joint differently from controller and Unreal refusing to have 2 variables with the same name
|
||||
// Joints get a "1" appended to their name -> refacto so it's "_Joint"
|
||||
return FSo100Actuators
|
||||
{
|
||||
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")),
|
||||
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")),
|
||||
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")),
|
||||
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")),
|
||||
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")),
|
||||
RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1"))
|
||||
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")),
|
||||
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")),
|
||||
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")),
|
||||
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")),
|
||||
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")),
|
||||
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1"))
|
||||
};
|
||||
}
|
||||
|
||||
@ -103,8 +118,8 @@ float URobotPilotSO100Component::GetDeltaSumBetweenActuatorValues(const FSo100Ac
|
||||
|
||||
double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const
|
||||
{
|
||||
auto const Control = RobotOwner->PhysicSceneProxy->GetActuatorValue(ActuatorName);
|
||||
auto const Joint = RobotOwner->PhysicSceneProxy->GetJointValue(ActuatorName.Append("1"));
|
||||
auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(ActuatorName);
|
||||
auto const Joint = RobotOwner->PhysicsSceneProxy->GetJointValue(ActuatorName.Append("1"));
|
||||
return Control -Joint;
|
||||
}
|
||||
|
||||
@ -143,9 +158,9 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
|
||||
|
||||
void URobotPilotSO100Component::NextAnimationState()
|
||||
{
|
||||
const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time;
|
||||
AnimationStartTime = CurrentSimulationTime;
|
||||
AnimStartRobotActuators = GetCurrentJointsFromPhysicScene();
|
||||
const float CurrentPhysicsEngineSceneTime = RobotOwner->PhysicsSceneProxy->GetMujocoData().time;
|
||||
AnimationStartTime = CurrentPhysicsEngineSceneTime;
|
||||
AnimStartRobotActuators = GetCurrentJointsFromPhysicsScene();
|
||||
|
||||
switch (CurrentAnimationState)
|
||||
{
|
||||
@ -226,7 +241,7 @@ void URobotPilotSO100Component::MoveToTarget()
|
||||
const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.);
|
||||
|
||||
// Set the target actuators values
|
||||
AnimTargetRobotActuators = GetCurrentJointsFromPhysicScene();
|
||||
AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene();
|
||||
AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend);
|
||||
|
||||
|
||||
@ -268,7 +283,7 @@ void URobotPilotSO100Component::MoveToDropZone()
|
||||
|
||||
AnimTargetRobotActuators = ActuatorsDropZone;
|
||||
AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.);
|
||||
AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw;
|
||||
AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicsScene().Jaw;
|
||||
CurrentAnimationState = 4;
|
||||
AnimationDuration = 3.f;
|
||||
}
|
||||
@ -287,7 +302,7 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
|
||||
const double AnimAlpha = FMath::Clamp((SimulationTime - AnimationStartTime) / AnimationDuration, 0., 1.);
|
||||
|
||||
// Need to wait for the joints to be in the right position before switching to next animation
|
||||
const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicScene());
|
||||
const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene());
|
||||
|
||||
// Alternative Animation completion event - checking for over-current
|
||||
if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold)
|
||||
@ -303,42 +318,42 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
|
||||
if (AnimAlpha >= 1. && DeltaSum <= .001) return true;
|
||||
|
||||
// Rotation
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
|
||||
AnimStartRobotActuators.Rotation,
|
||||
AnimTargetRobotActuators.Rotation,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Pitch
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp(
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp(
|
||||
AnimStartRobotActuators.Pitch,
|
||||
AnimTargetRobotActuators.Pitch,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Elbow
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp(
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp(
|
||||
AnimStartRobotActuators.Elbow,
|
||||
AnimTargetRobotActuators.Elbow,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// WristPitch
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp(
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp(
|
||||
AnimStartRobotActuators.WristPitch,
|
||||
AnimTargetRobotActuators.WristPitch,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// WristRoll
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp(
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp(
|
||||
AnimStartRobotActuators.WristRoll,
|
||||
AnimTargetRobotActuators.WristRoll,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Jaw
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
|
||||
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
|
||||
AnimStartRobotActuators.Jaw,
|
||||
AnimTargetRobotActuators.Jaw,
|
||||
AnimAlpha
|
||||
|
||||
Reference in New Issue
Block a user