From 6018e4295fd08fe8153717b75bb30c2ddeb9ff75 Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Fri, 18 Sep 2020 18:26:58 +0900 Subject: [PATCH] Bug fix in Micom Device - fix rotation direction for ROS2 message there was bug for conversion between Left-handed and right-handed --- Assets/Scripts/Devices/MicomSensor.cs | 13 ++++++++----- Assets/Scripts/Devices/Modules/Motor.cs | 3 +-- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs index df455b6e..4ccffd65 100644 --- a/Assets/Scripts/Devices/MicomSensor.cs +++ b/Assets/Scripts/Devices/MicomSensor.cs @@ -342,8 +342,10 @@ public bool UpdateOdom() var odom = micomSensorData.Odom; if ((odom != null) && (motorLeft != null && motorRight != null)) { - var angularVelocityLeft = motorLeft.GetCurrentVelocity(); - var angularVelocityRight = motorRight.GetCurrentVelocity(); + // Set revsered value due to differnt direction + // Left-handed -> Right-handed direction of rotation + var angularVelocityLeft = -motorLeft.GetCurrentVelocity(); + var angularVelocityRight = -motorRight.GetCurrentVelocity(); odom.AngularVelocityLeft = angularVelocityLeft * Mathf.Deg2Rad; odom.AngularVelocityRight = angularVelocityRight * Mathf.Deg2Rad; @@ -387,12 +389,13 @@ private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelo { if (motorLeft != null && motorRight != null) { - motorLeft.SetVelocityTarget(angularVelocityLeft); - motorRight.SetVelocityTarget(angularVelocityRight); + // Set revsered value due to differnt direction + // Right-handed -> Left-handed direction of rotation + motorLeft.SetVelocityTarget(-angularVelocityLeft); + motorRight.SetVelocityTarget(-angularVelocityRight); } } - public Pose GetPartsPose(in string targetPartsName) { if (partsPoseMapTable.TryGetValue(targetPartsName, out var targetPartsPose)) diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index 7374f656..9178b69f 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -72,9 +72,8 @@ public void SetVelocityTarget(in float targetAngularVelocity) // name, currentVelocity, targetAngularVelocity, cmdForce, rigidBody.maxAngularVelocity); // JointMotor.targetVelocity angular velocity in degrees per second. - // Set revsered value due to differnt direction between ROS2/SDF(Right-handed) var motor = joint.motor; - motor.targetVelocity = -targetAngularVelocity; + motor.targetVelocity = targetAngularVelocity; motor.force = Mathf.Round(cmdForce); if (targetAngularVelocity == 0)