Run mj_forward (kinematics + dynamics, no integration) and return observations. Lets a client write qpos / qvel then read consistent derived state (xpos, sensors, contacts, ...) without advancing time.
| 1868 | // observations. Lets a client write qpos / qvel then read consistent |
| 1869 | // derived state (xpos, sensors, contacts, ...) without advancing time. |
| 1870 | TSharedPtr<FJsonObject> FURLabRpcDispatcher::HandleForward(const TSharedPtr<FJsonObject>& /*Req*/) |
| 1871 | { |
| 1872 | AAMjManager* Mgr = OwnerMgr.Get(); |
| 1873 | if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) |
| 1874 | { |
| 1875 | return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); |
| 1876 | } |
| 1877 | |
| 1878 | mjModel* m = Mgr->PhysicsEngine->GetModel(); |
| 1879 | mjData* d = Mgr->PhysicsEngine->GetData(); |
| 1880 | |
| 1881 | { |
| 1882 | FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); |
| 1883 | mj_forward(m, d); |
| 1884 | } |
| 1885 | |
| 1886 | TSharedPtr<FJsonObject> Reply = MakeShared<FJsonObject>(); |
| 1887 | Reply->SetStringField(TEXT("op"), TEXT("forward_ok")); |
| 1888 | Reply->SetNumberField(TEXT("time"), d->time); |
| 1889 | Reply->SetNumberField(TEXT("step"), StepCounter.load(std::memory_order_relaxed)); |
| 1890 | AppendClockFields(Reply, d->time); |
| 1891 | TSharedPtr<FJsonObject> Obs = BuildStepObservations(Mgr, m, d, ActiveObservationLevel); |
| 1892 | if (Obs.IsValid()) |
| 1893 | Reply->SetObjectField(TEXT("per_articulation"), Obs); |
| 1894 | return Reply; |
| 1895 | } |
| 1896 | |
| 1897 | TSharedPtr<FJsonObject> FURLabRpcDispatcher::HandleSetMode(const TSharedPtr<FJsonObject>& Req) |
| 1898 | { |