RRJointComponent.h

Go to the documentation of this file.

 
#pragma once
 
 
 
// UE
 
#include "Components/ActorComponent.h"
 
#include "CoreMinimal.h"
 
 
 
// RapyutaSimulationPlugins
 
#include "Core/RRStaticMeshComponent.h"
 
#include "Tools/RRROS2TFPublisher.h"
 
 
 
#include "RRJointComponent.generated.h"
 
 
 
#define RAPYUTA_JOINT_DEBUG (0)
 
 
 
DECLARE_DYNAMIC_DELEGATE(FJointCallback);
 
 
 
 
enum class ERRJointControlType : uint8
 
{
 
    POSITION , 
 
    VELOCITY , 
 
    EFFORT , 
 
};
 
 
 
class RAPYUTASIMULATIONPLUGINS_API URRJointComponent : public USceneComponent
 
{
 
 
 
public:
 
    // Sets default values for this component's properties
 
    URRJointComponent();
 
 
 
protected:
 
    virtual void PoseFromArray(const TArray<float>& InPose, FVector& OutPosition, FRotator& OutOrientation);
 
    virtual void VelocityFromArray(const TArray<float>& InVelocity, FVector& OutLinearVelocity, FVector& OutAngularVelocity);
 
 
 
public:
 
    virtual bool IsValid();
 
 
 
    virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
 
 
 
    virtual void InitializeComponent() override;
 
 
 
    virtual void SetVelocity(const FVector& InLinearVelocity, const FVector& InAngularVelocity);
 
 
 
    virtual void SetVelocityTarget(const FVector& InLinearVelocity, const FVector& InAngularVelocity);
 
 
 
    virtual void SetSingleLinearVelocityTarget(const float Input);
 
 
 
    virtual void SetSingleAngularVelocityTarget(const float Input);
 
 
 
    virtual void SetVelocityTargetWithDelegates(const FVector& InLinearVelocity,
 
                                                const FVector& InAngularVelocity,
 
                                                const FJointCallback& InOnControlSuccessDelegate,
 
                                                const FJointCallback& InOnControlFailDelegate,
 
                                                const float InLinearTolerance = -1.0,
 
                                                const float InAngularTolerance = -1.0,
 
                                                const float InTimeOut = -1.0);
 
 
 
    virtual void SetSingleLinearVelocityTargetWithDelegates(const float Input,
 
                                                            const FJointCallback& InOnControlSuccessDelegate,
 
                                                            const FJointCallback& InOnControlFailDelegate,
 
                                                            const float InLinearTolerance = -1.0,
 
                                                            const float InTimeOut = -1.0);
 
 
 
    virtual void SetSingleAngularVelocityTargetWithDelegates(const float Input,
 
                                                             const FJointCallback& InOnControlSuccessDelegate,
 
                                                             const FJointCallback& InOnControlFailDelegate,
 
                                                             const float InAngularTolerance = -1.0,
 
                                                             const float InTimeOut = -1.0);
 
 
 
    virtual void SetVelocityWithArray(const TArray<float>& InVelocity);
 
 
 
    virtual void SetVelocityTargetWithArray(const TArray<float>& InVelocity);
 
 
 
    virtual void SetVelocityTargetWithArrayWithDelegates(const TArray<float>& InVelocity,
 
                                                         const FJointCallback& InOnControlSuccessDelegate,
 
                                                         const FJointCallback& InOnControlFailDelegate,
 
                                                         const float InLinearTolerance = -1.0,
 
                                                         const float InAngularTolerance = -1.0,
 
                                                         const float InTimeOut = -1.0);
 
 
 
    virtual bool HasReachedVelocityTarget(const float InLinearTolerance = -1.f, const float InAngularTolerance = -1.f);
 
 
 
    virtual void SetPose(const FVector& InPosition, const FRotator& InOrientation);
 
 
 
    virtual void SetPoseTarget(const FVector& InPosition, const FRotator& InOrientation);
 
 
 
    virtual void SetSinglePositionTarget(const float Input);
 
 
 
    virtual void SetSingleOrientationTarget(const float Input);
 
 
 
    virtual void SetPoseTargetWithDelegates(const FVector& InPosition,
 
                                            const FRotator& InOrientation,
 
                                            const FJointCallback& InOnControlSuccessDelegate,
 
                                            const FJointCallback& InOnControlFailDelegate,
 
                                            const float InPositionTolerance = -1.0,
 
                                            const float InOrientationTolerance = -1.0,
 
                                            const float InTimeOut = -1.0);
 
 
 
    virtual void SetSinglePositionTargetWithDelegates(const float Input,
 
                                                      const FJointCallback& InOnControlSuccessDelegate,
 
                                                      const FJointCallback& InOnControlFailDelegate,
 
                                                      const float InPositionTolerance = -1.0,
 
                                                      const float InTimeOut = -1.0);
 
 
 
    virtual void SetSingleOrientationTargetWithDelegates(const float Input,
 
                                                         const FJointCallback& InOnControlSuccessDelegate,
 
                                                         const FJointCallback& InOnControlFailDelegate,
 
                                                         const float InOrientationTolerance = -1.0,
 
                                                         const float InTimeOut = -1.0);
 
 
 
    virtual bool HasReachedPoseTarget(const float InPositionTolerance = -1.f, const float InOrientationTolerance = -1.f);
 
 
 
    virtual void SetPoseWithArray(const TArray<float>& InPose);
 
 
 
    virtual void SetPoseTargetWithArray(const TArray<float>& InPose);
 
 
 
    virtual void SetPoseTargetWithArrayWithDelegates(const TArray<float>& InPose,
 
                                                     const FJointCallback& InOnControlSuccessDelegate,
 
                                                     const FJointCallback& InOnControlFailDelegate,
 
                                                     const float InPositionTolerance = -1.0,
 
                                                     const float InOrientationTolerance = -1.0,
 
                                                     const float InTimeOut = -1.0);
 
 
 
    virtual void Teleport(const FVector& InPosition, const FRotator& InOrientation);
 
 
 
 
    virtual void MoveToInitPose();
 
 
 
 
    FVector LinearVelocity = FVector::ZeroVector;
 
 
 
 
    FVector AngularVelocity = FVector::ZeroVector;
 
 
 
 
    FVector LinearVelocityTarget = FVector::ZeroVector;
 
 
 
 
    FVector AngularVelocityTarget = FVector::ZeroVector;
 
 
 
 
    FVector PositionTarget = FVector::ZeroVector;
 
 
 
 
    FRotator OrientationTarget = FRotator::ZeroRotator;
 
 
 
 
    FVector Position = FVector::ZeroVector;
 
 
 
 
    FRotator Orientation = FRotator::ZeroRotator;
 
 
 
 
 
    float PositionTolerance = 1.f;
 
 
 
 
 
    float OrientationTolerance = 1.f;
 
 
 
 
 
    float LinearVelocityTolerance = 10.f;
 
 
 
 
 
    float AngularVelocityTolerance = 10.f;
 
 
 
 
    UStaticMeshComponent* ParentLink;
 
 
 
 
    UStaticMeshComponent* ChildLink;
 
 
 
 
    ERRJointControlType ControlType = ERRJointControlType::POSITION;
 
 
 
 
 
    uint8 LinearDOF = 0;
 
 
 
 
 
    uint8 RotationalDOF = 1;
 
 
 
 
 
 
    FVector PositionMax = FVector(1000, 1000, 1000);
 
 
 
 
 
    FVector PositionMin = FVector(-1000, -1000, -1000);
 
 
 
 
 
    FRotator OrientationMax = FRotator(180.f);
 
 
 
 
 
    FRotator OrientationMin = FRotator(-180, -180, -180);
 
 
 
 
 
 
    FVector LinearVelMax = FVector(1000, 1000, 1000);
 
 
 
 
 
    FVector AngularVelMax = FVector(180.f);
 
 
 
 
    bool bLimitRoll = true;
 
 
 
 
    bool bLimitPitch = true;
 
 
 
 
    bool bLimitYaw = true;
 
 
 
 
    FVector InitialPosition = FVector::ZeroVector;
 
 
 
 
    FRotator InitialOrientation = FRotator::ZeroRotator;
 
 
 
 
    FTransform GetJointToChildLink()
 
    {
 
        return JointToChildLink;
 
    }
 
 
 
 
    FTransform GetParentLinkToJoint()
 
    {
 
        return ParentLinkToJoint;
 
    }
 
 
 
protected:
 
 
    virtual void UpdateState(const float DeltaTime);
 
 
 
 
    virtual void UpdateControl(const float DeltaTime);
 
 
 
 
    FTransform JointToChildLink = FTransform::Identity;
 
 
 
 
    FTransform ParentLinkToJoint = FTransform::Identity;
 
 
 
 
 
    bool bMovingToTargetPose = false;
 
 
 
 
 
    bool bMovingToTargetVelocity = false;
 
 
 
    virtual void SetDelegates(const FJointCallback& InOnControlSuccessDelegate,
 
                              const FJointCallback& InOnControlFailDelegate,
 
                              const float InPositionTolerance = -1.0,
 
                              const float InOrientationTolerance = -1.0,
 
                              const float InLinearTolerance = -1.0,
 
                              const float InAngularTolerance = -1.0,
 
                              const float InTimeOut = -1.0f);
 
 
 
 
 
    FJointCallback OnControlSuccessDelegate;
 
 
 
 
 
    FJointCallback OnControlFailDelegate;
 
 
 
 
    float ControlStartTime = 0.f;
 
 
 
 
 
 
 
    float ControlTimeout = -1.0f;
 
 
 
 
    virtual void ResetControl();
 
};