Merge pull request #41 from snozawa/fix_frame_of_accel_sensor

Fix accel sensor frame
fkanehiro · Feb 9, 2015 · 9f3388e · 9f3388e
2 parents 7e9b649 + 5b101d5
commit 9f3388e
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/hrplib/hrpModel/ForwardDynamics.cpp b/hrplib/hrpModel/ForwardDynamics.cpp
@@ -142,7 +142,7 @@ void ForwardDynamics::updateAccelSensor(AccelSensor* sensor)
     Vector3 o_Agsens(x[0](1), x[1](1), x[2](1));
     o_Agsens += g;
 
-    sensor->dv.noalias() = link->R.transpose() * o_Agsens;
+    sensor->dv.noalias() = sensor->localR.transpose() * link->R.transpose() * o_Agsens;
 }