Method HandleSetTwist

Source/URLab/Private/Bridge/RpcDispatcher.cpp:2538–2592

// =============================================================================

TSharedPtr<FJsonObject> FURLabRpcDispatcher::HandleSetTwist(const TSharedPtr<FJsonObject>& Req)
{
	AAMjManager* Mgr = OwnerMgr.Get();
	if (!Mgr)
		return MakeError(TEXT("not_ready"), TEXT("Manager missing"));

	FString ArtName;
	if (!Req->TryGetStringField(TEXT("articulation"), ArtName))
		return MakeError(TEXT("missing_field"), TEXT("set_twist requires 'articulation'"));

	AMjArticulation* Art = Mgr->GetArticulation(ArtName);
	if (!Art)
		return MakeError(TEXT("unknown_articulation"), ArtName);

	UMjTwistController* TC = Art->FindComponentByClass<UMjTwistController>();
	if (!TC)
		return MakeError(TEXT("no_twist_controller"),
			FString::Printf(TEXT("Articulation '%s' has no UMjTwistController"), *ArtName));

	// Wire format mirrors how the bridge already reads twist: linear is
	// (vx, vy, _) m/s, angular is (_, _, yaw_rate) rad/s. Tuple slots
	// beyond the ones used are accepted but ignored.
	auto ReadAxis = [](const TArray<TSharedPtr<FJsonValue>>* Arr, int32 Idx, float& Out) {
		if (Arr && Arr->IsValidIndex(Idx))
			Out = (float)(*Arr)[Idx]->AsNumber();
	};

	float Vx = 0.f, Vy = 0.f, YawRate = 0.f;
	const TArray<TSharedPtr<FJsonValue>>* LinArr = nullptr;
	const TArray<TSharedPtr<FJsonValue>>* AngArr = nullptr;
	Req->TryGetArrayField(TEXT("linear"), LinArr);
	Req->TryGetArrayField(TEXT("angular"), AngArr);
	ReadAxis(LinArr, 0, Vx);
	ReadAxis(LinArr, 1, Vy);
	ReadAxis(AngArr, 2, YawRate);

	TC->SetTwist(Vx, Vy, YawRate);

	TSharedPtr<FJsonObject> Reply = MakeShared<FJsonObject>();
	Reply->SetStringField(TEXT("op"), TEXT("set_twist_ok"));
	Reply->SetStringField(TEXT("articulation"), ArtName);
	{
		TArray<TSharedPtr<FJsonValue>> L;
		L.Add(MakeShared<FJsonValueNumber>(Vx));
		L.Add(MakeShared<FJsonValueNumber>(Vy));
		L.Add(MakeShared<FJsonValueNumber>(0.0));
		Reply->SetArrayField(TEXT("linear"), L);
		TArray<TSharedPtr<FJsonValue>> A;
		A.Add(MakeShared<FJsonValueNumber>(0.0));
		A.Add(MakeShared<FJsonValueNumber>(0.0));
		A.Add(MakeShared<FJsonValueNumber>(YawRate));
		Reply->SetArrayField(TEXT("angular"), A);
	}
	return Reply;
}

// =============================================================================
// set_qpos — manager-required runtime write to a single articulation's qpos.