FT - SO100-PilotComponent
+ Grab Sequence Retrac->Find Target->Rotate towards target->Extend->Grab->Drop + Can work with any target on the right side (left side bugged - I know why)
This commit is contained in:
Jb win
parent
42196c7680
commit
73b0aa12bf
Binary file not shown.
Binary file not shown.
@ -4,10 +4,12 @@
|
||||
#include "Kismet/KismetMathLibrary.h"
|
||||
#include "Kismet/KismetSystemLibrary.h"
|
||||
#include "Robot/RobotPawn.h"
|
||||
#include "Serialization/ShaderKeyGenerator.h"
|
||||
|
||||
URobotPilotSO100Component::URobotPilotSO100Component()
|
||||
{
|
||||
|
||||
PrimaryComponentTick.bCanEverTick = true;
|
||||
PrimaryComponentTick.bStartWithTickEnabled = true;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::BeginPlay()
|
||||
@ -18,20 +20,30 @@ void URobotPilotSO100Component::BeginPlay()
|
||||
void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType,
|
||||
FActorComponentTickFunction* ThisTickFunction)
|
||||
{
|
||||
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
// Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
// const auto Joints = GetCurrentJointsFromPhysicScene();
|
||||
// const auto Controls = GetCurrentControlsFromPhysicScene();
|
||||
// UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw);
|
||||
}
|
||||
|
||||
|
||||
void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn)
|
||||
{
|
||||
// Set Base Values
|
||||
AnimStartRobotActuators = GetCurrentActuatorsFromPhysicScene();
|
||||
TargetTransform = TargetTransformIn;
|
||||
NextAnimationState();
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::PrintActuators() const
|
||||
|
||||
|
||||
void URobotPilotSO100Component::PrintCurrentActuators() const
|
||||
{
|
||||
const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = GetCurrentActuatorsFromPhysicScene();
|
||||
PrintActuators(GetCurrentControlsFromPhysicScene());
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::PrintActuators(FSo100Actuators Actuators)
|
||||
{
|
||||
const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = Actuators;
|
||||
UE_LOG(LogTemp, Display, TEXT("Actuators Rotation %f | Pitch %f | Elbow %f | WristPitch %f | WristRoll %f | Jaw %f"),
|
||||
Rotation,
|
||||
Pitch,
|
||||
@ -42,16 +54,73 @@ void URobotPilotSO100Component::PrintActuators() const
|
||||
);
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentActuatorsFromPhysicScene() const
|
||||
void URobotPilotSO100Component::DisableAnim()
|
||||
{
|
||||
AnimationStartTime = 0;
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const
|
||||
{
|
||||
return FSo100Actuators
|
||||
{
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Rotation),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Pitch),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Elbow),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristPitch),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristRoll),
|
||||
RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Jaw),
|
||||
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),
|
||||
};
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicScene() 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"))
|
||||
};
|
||||
}
|
||||
|
||||
float URobotPilotSO100Component::GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B)
|
||||
{
|
||||
float DeltaSum = 0;
|
||||
|
||||
DeltaSum += FMath::Abs(A.Rotation) - FMath::Abs(B.Rotation);
|
||||
DeltaSum += FMath::Abs(A.Pitch) - FMath::Abs(B.Pitch);
|
||||
DeltaSum += FMath::Abs(A.Elbow) - FMath::Abs(B.Elbow);
|
||||
DeltaSum += FMath::Abs(A.WristPitch) - FMath::Abs(B.WristPitch);
|
||||
DeltaSum += FMath::Abs(A.WristRoll) - FMath::Abs(B.WristRoll);
|
||||
DeltaSum += FMath::Abs(A.Jaw) - FMath::Abs(B.Jaw);
|
||||
|
||||
return DeltaSum;
|
||||
}
|
||||
|
||||
double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const
|
||||
{
|
||||
auto const Control = RobotOwner->PhysicSceneProxy->GetActuatorValue(ActuatorName);
|
||||
auto const Joint = RobotOwner->PhysicSceneProxy->GetJointValue(ActuatorName.Append("1"));
|
||||
return Control -Joint;
|
||||
}
|
||||
|
||||
FSo100Actuators URobotPilotSO100Component::LerpActuators(
|
||||
const FSo100Actuators& A,
|
||||
const FSo100Actuators& B,
|
||||
const float Alpha
|
||||
)
|
||||
{
|
||||
return FSo100Actuators{
|
||||
FMath::Lerp( A.Rotation, B.Rotation, Alpha),
|
||||
FMath::Lerp( A.Pitch, B.Pitch, Alpha),
|
||||
FMath::Lerp( A.Elbow, B.Elbow, Alpha),
|
||||
FMath::Lerp( A.WristPitch, B.WristPitch, Alpha),
|
||||
FMath::Lerp( A.WristRoll, B.WristRoll, Alpha),
|
||||
FMath::Lerp( A.Jaw, B.Jaw, Alpha),
|
||||
};
|
||||
}
|
||||
|
||||
@ -59,13 +128,14 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
|
||||
{
|
||||
// Do anything that should be done after a mj_step update
|
||||
|
||||
if (AnimationStartTime >= 0)
|
||||
if (AnimationStartTime > 0)
|
||||
{
|
||||
// Could be moved to if statement, but it becomes confusing - so let's keep a variable
|
||||
const bool bHasFinishedAnimation = AnimateActuators(SimulationTime);
|
||||
|
||||
if (bHasFinishedAnimation && CurrentAnimationState < MaxAnimationState)
|
||||
{
|
||||
AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode
|
||||
NextAnimationState();
|
||||
}
|
||||
}
|
||||
@ -75,19 +145,22 @@ void URobotPilotSO100Component::NextAnimationState()
|
||||
{
|
||||
const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time;
|
||||
AnimationStartTime = CurrentSimulationTime;
|
||||
AnimStartRobotActuators = GetCurrentJointsFromPhysicScene();
|
||||
|
||||
switch (CurrentAnimationState)
|
||||
{
|
||||
case 0:
|
||||
return RotateToTarget();
|
||||
case 1:
|
||||
return CloseJaw();
|
||||
return MoveToTarget();
|
||||
case 2:
|
||||
return MoveToDropZone();
|
||||
return CloseJaw();
|
||||
case 3:
|
||||
return OpenJaw();
|
||||
return MoveToDropZone();
|
||||
case 4:
|
||||
return OpenJaw();
|
||||
case 5:
|
||||
return OpenJaw();
|
||||
default:
|
||||
return BasePose();
|
||||
}
|
||||
@ -96,98 +169,165 @@ void URobotPilotSO100Component::NextAnimationState()
|
||||
void URobotPilotSO100Component::BasePose()
|
||||
{
|
||||
CurrentAnimationState = 0;
|
||||
AnimationDuration = 1.5f;
|
||||
AnimationDuration = 1.f;
|
||||
AnimTargetRobotActuators = ActuatorsRestPosition;
|
||||
// AnimTargetRobotActuators = ActuatorsMaxExtendPosition;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::RotateToTarget()
|
||||
{
|
||||
CurrentAnimationState = 1;
|
||||
AnimationDuration = 1.f;
|
||||
|
||||
// Compute Pivot Point World Location
|
||||
const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform();
|
||||
const FVector PivotWorldLocation = WorldTransform.GetTranslation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
|
||||
|
||||
const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
|
||||
|
||||
// FVector::CosineAngle2D()
|
||||
const auto Angle = UKismetMathLibrary::FindLookAtRotation(
|
||||
WorldTransform.GetRotation().GetForwardVector(),
|
||||
TargetTransform.GetLocation() - PivotWorldLocation
|
||||
// Find Yaw Rotation in degree - cache it for distance to target computation
|
||||
RotationToTarget = UKismetMathLibrary::FindLookAtRotation(
|
||||
PivotWorldLocation,
|
||||
TargetTransform.GetLocation()
|
||||
);
|
||||
|
||||
const auto Cosine = WorldTransform.GetRotation().GetForwardVector().CosineAngle2D(TargetTransform.GetLocation() - PivotWorldLocation);
|
||||
|
||||
UE_LOG(LogTemp, Display, TEXT("Rotation %f, %f, %f"), Angle.Roll, Angle.Pitch, Angle.Yaw);
|
||||
UE_LOG(LogTemp, Display, TEXT("Cosine %f"), Cosine);
|
||||
// Convert to radians
|
||||
// Looks like we are not in the same referential hence the -1 !
|
||||
// reduce Yaw to not have the fixed jaw colliding with the shape
|
||||
const auto ActuatorRotation = RotationToTarget.Yaw * 0.9 / 180.0f * -PI;
|
||||
|
||||
// Start the animation
|
||||
AnimTargetRobotActuators = AnimStartRobotActuators;
|
||||
AnimTargetRobotActuators.Rotation = ActuatorRotation;
|
||||
CurrentAnimationState = 1;
|
||||
AnimationDuration = .7f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::MoveToTarget()
|
||||
{
|
||||
// Get Pivot World
|
||||
const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform();
|
||||
const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
|
||||
|
||||
// Rotate Jaw offset towards target
|
||||
// Get pure 2d rotation
|
||||
RotationToTarget.Pitch = 0;
|
||||
RotationToTarget.Roll = 0;
|
||||
const auto JawOffsetToPivot = JawOffset - PivotOffset;
|
||||
const auto JawPositionWorld = RotationToTarget.RotateVector(JawOffsetToPivot) + PivotWorldLocation;
|
||||
const auto Distance = FVector::Distance(JawPositionWorld, TargetTransform.GetLocation());
|
||||
|
||||
DrawDebugLine(
|
||||
this->GetWorld(),
|
||||
JawPositionWorld,
|
||||
JawPositionWorld + FVector::UpVector * 100,
|
||||
FColor::Blue,
|
||||
true
|
||||
);
|
||||
|
||||
const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.);
|
||||
// UE_LOG(LogTemp, Log, TEXT("Distance: %f - AlphaExtend: %f"), Distance, AlphaExtend);
|
||||
|
||||
// Set the target actuators values
|
||||
AnimTargetRobotActuators = GetCurrentJointsFromPhysicScene();
|
||||
AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend);
|
||||
|
||||
|
||||
AnimTargetRobotActuators.Rotation = AnimStartRobotActuators.Rotation;
|
||||
PrintActuators(AnimTargetRobotActuators);
|
||||
|
||||
// Start the animation
|
||||
CurrentAnimationState = 2;
|
||||
AnimationDuration = 1.f;
|
||||
|
||||
DrawDebugLine(
|
||||
this->GetWorld(),
|
||||
PivotWorldLocation + 100 * FVector::UpVector,
|
||||
PivotWorldLocation,
|
||||
FColor::Blue,
|
||||
true);
|
||||
JawPositionWorld,
|
||||
JawPositionWorld + (TargetTransform.GetLocation() - JawPositionWorld).GetSafeNormal() * Distance,
|
||||
FColor::Green,
|
||||
true
|
||||
);
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::CloseJaw()
|
||||
{
|
||||
// Here we need overcurrent detection - not here actually
|
||||
AnimTargetRobotActuators.Jaw = ClosedJaw;
|
||||
|
||||
// Start the animation
|
||||
bDetectOverCurrent = true;
|
||||
CurrentAnimationState = 3;
|
||||
AnimationDuration = 1.6f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::MoveToDropZone()
|
||||
{
|
||||
const auto DropZoneActuatorsSettings = FMath::RandBool() ? ActuatorsLeftDropZone : ActuatorsRightDropZone;
|
||||
|
||||
AnimTargetRobotActuators = ActuatorsDropZone;
|
||||
AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.);
|
||||
AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw;
|
||||
CurrentAnimationState = 4;
|
||||
AnimationDuration = 3.f;
|
||||
}
|
||||
|
||||
void URobotPilotSO100Component::OpenJaw()
|
||||
{
|
||||
AnimTargetRobotActuators.Jaw = OpenedJaw;
|
||||
CurrentAnimationState = 5;
|
||||
AnimationDuration = 0.6f;
|
||||
}
|
||||
|
||||
bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) const
|
||||
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());
|
||||
|
||||
// Alternative Animation completion event - checking for over-current
|
||||
if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold)
|
||||
{
|
||||
bDetectOverCurrent = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// UE_LOG(LogTemp, Log, TEXT("AnimationAlpha: %f - Delta: %f"), AnimAlpha, Delta);
|
||||
|
||||
// Stop the animation if we reached the target
|
||||
if (AnimAlpha >= 1.) return true;
|
||||
if (AnimAlpha >= 1. && DeltaSum <= .001) return true;
|
||||
|
||||
// Rotation
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Rotation, FMath::Lerp(
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
|
||||
AnimStartRobotActuators.Rotation,
|
||||
AnimTargetRobotActuators.Rotation,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Pitch
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Pitch, FMath::Lerp(
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp(
|
||||
AnimStartRobotActuators.Pitch,
|
||||
AnimTargetRobotActuators.Pitch,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Elbow
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Elbow, FMath::Lerp(
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp(
|
||||
AnimStartRobotActuators.Elbow,
|
||||
AnimTargetRobotActuators.Elbow,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// WristPitch
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristPitch, FMath::Lerp(
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp(
|
||||
AnimStartRobotActuators.WristPitch,
|
||||
AnimTargetRobotActuators.WristPitch,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// WristRoll
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristRoll, FMath::Lerp(
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp(
|
||||
AnimStartRobotActuators.WristRoll,
|
||||
AnimTargetRobotActuators.WristRoll,
|
||||
AnimAlpha
|
||||
));
|
||||
|
||||
// Jaw
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Jaw, FMath::Lerp(
|
||||
RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
|
||||
AnimStartRobotActuators.Jaw,
|
||||
AnimTargetRobotActuators.Jaw,
|
||||
AnimAlpha
|
||||
|
||||
@ -41,40 +41,64 @@ public:
|
||||
void SetTarget(const FTransform& TargetTransformIn);
|
||||
private:
|
||||
FTransform TargetTransform;
|
||||
|
||||
|
||||
//---------------------
|
||||
//------- DEBUG -------
|
||||
//---------------------
|
||||
public:
|
||||
UFUNCTION(BlueprintCallable)
|
||||
void PrintActuators() const;
|
||||
void PrintCurrentActuators() const;
|
||||
static void PrintActuators(FSo100Actuators Actuators);
|
||||
|
||||
UFUNCTION(BlueprintCallable)
|
||||
void DisableAnim();
|
||||
|
||||
private:
|
||||
// SO100 Controls by name
|
||||
FString Ctrl_Rotation = FString("Rotation");
|
||||
FString Ctrl_Pitch = FString("Pitch");
|
||||
FString Ctrl_Elbow = FString("Elbow");
|
||||
FString Ctrl_WristPitch = FString("Wrist_Pitch");
|
||||
FString Ctrl_WristRoll = FString("Wrist_Roll");
|
||||
FString Ctrl_Jaw = FString("Jaw");
|
||||
FString Actuator_Rotation = FString("Rotation");
|
||||
FString Actuator_Pitch = FString("Pitch");
|
||||
FString Actuator_Elbow = FString("Elbow");
|
||||
FString Actuator_WristPitch = FString("Wrist_Pitch");
|
||||
FString Actuator_WristRoll = FString("Wrist_Roll");
|
||||
FString Actuator_Jaw = FString("Jaw");
|
||||
|
||||
// SO100 Static Variables
|
||||
FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041};
|
||||
FVector JawOffset = FVector{23, 2, 9};
|
||||
float MaxReach = 20.757929; // fixed_jaw_pad_3 ForwardVectorLength Delta between Rest and MaxExtend
|
||||
|
||||
// Actuators Joints and Controls are expressed in doubles
|
||||
double ClosedJaw = 0.18;
|
||||
double OpenedJaw = -2.0;
|
||||
|
||||
/**
|
||||
* Query the physic proxy on the RobotOwner to get the SO100 actuators values
|
||||
* @return
|
||||
*/
|
||||
FSo100Actuators GetCurrentActuatorsFromPhysicScene() const;
|
||||
FSo100Actuators GetCurrentControlsFromPhysicScene() const;
|
||||
FSo100Actuators GetCurrentJointsFromPhysicScene() const;
|
||||
double GetControlJointDeltaForActuator(FString ActuatorName) const;
|
||||
static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B);
|
||||
static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha);
|
||||
|
||||
// Called after every physic step
|
||||
virtual void PostPhysicStepUpdate(const float SimulationTime) override;
|
||||
bool AnimateActuators(float SimulationTime) const; // Bound to the PhysicProxy post-update delegate
|
||||
bool AnimateActuators(float SimulationTime); // Bound to the PhysicProxy post-update delegate
|
||||
FSo100Actuators CurrentRobotActuators; // This will be updated by the post-physic delegate
|
||||
FSo100Actuators AnimStartRobotActuators;
|
||||
FSo100Actuators AnimTargetRobotActuators;
|
||||
FRotator RotationToTarget = FRotator::ZeroRotator;
|
||||
|
||||
// ------------------------
|
||||
// ----- OVER-CURRENT -----
|
||||
// ------------------------
|
||||
bool bDetectOverCurrent = false;
|
||||
const float OverCurrentThreshold = 0.15;
|
||||
|
||||
// Quick and dirty sequence of moves
|
||||
// -1 -> Start Game, extended
|
||||
// 0 -> Retract - base pose
|
||||
// 1 -> Rotate towards goal
|
||||
// 1 -> Rotate towards target
|
||||
// 2 -> Move to target
|
||||
// 3 -> Close the jaw
|
||||
// 4 -> Go to drop zone
|
||||
@ -85,6 +109,7 @@ private:
|
||||
|
||||
void BasePose();
|
||||
void RotateToTarget();
|
||||
void MoveToTarget();
|
||||
void CloseJaw();
|
||||
void MoveToDropZone();
|
||||
void OpenJaw();
|
||||
@ -104,32 +129,23 @@ private:
|
||||
};
|
||||
|
||||
FSo100Actuators ActuatorsMaxExtendPosition {
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
-1.56,
|
||||
-1.045
|
||||
0.0000001,
|
||||
0.081027,
|
||||
-0.01707,
|
||||
-0.075,
|
||||
1.469020,
|
||||
-1.389073
|
||||
};
|
||||
|
||||
FSo100Actuators ActuatorsLeftDropZone {
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
-1.56,
|
||||
-1.045
|
||||
FSo100Actuators ActuatorsDropZone {
|
||||
PI / 2,
|
||||
-2.17,
|
||||
0.805,
|
||||
1.345,
|
||||
1.61,
|
||||
0
|
||||
};
|
||||
|
||||
FSo100Actuators ActuatorsRightDropZone {
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
0.,
|
||||
-1.56,
|
||||
-1.045
|
||||
};
|
||||
|
||||
// Tick where it can have targets
|
||||
// Open Claw (ClawIndex)
|
||||
// Presets to move certain joints into certain positions -> HardCode
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user