From d0cabcf7dbf004f234bc0d9ba9dfe719a97d4e66 Mon Sep 17 00:00:00 2001
From: Erik Frey <erikfrey@google.com>
Date: Wed, 8 Nov 2023 16:14:53 -0800
Subject: [PATCH] Only apply torquescale once, when setting efc_pos.

PiperOrigin-RevId: 580694671
Change-Id: Ic0e2ee773d65d4fd92f4b4102cdfdd040516680a
---
 src/engine/engine_core_constraint.c | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c
index 29f884cd..d1cb4a6c 100644
--- a/src/engine/engine_core_constraint.c
+++ b/src/engine/engine_core_constraint.c
@@ -1310,16 +1310,8 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int
 
   case mjCNSTR_EQUALITY:
     if (m->eq_type[id] == mjEQ_WELD) {
-      mjtNum rotlinratio = m->eq_data[mjNEQDATA*id+10];
-      mjtNum efc_pos[6];
-
-      // copy translational residual
-      mju_copy3(efc_pos, d->efc_pos+i);
-
-      // multiply orientations by torquescale
-      mju_scl3(efc_pos+3, d->efc_pos+i+3, rotlinratio);
       *dim = 6;
-      *pos = mju_norm(efc_pos, 6);
+      *pos = mju_norm(d->efc_pos+i, 6);
     } else if (m->eq_type[id] == mjEQ_CONNECT) {
       *dim = 3;
       *pos = mju_norm(d->efc_pos+i, 3);