[Manipulator simulation]Joint torque precision

[yuncongwan]

Hello,

I'm trying to simulate a robot arm in Pybullet and want to get joint torque of each joint.

I already got a simulated data using another simulation tool (which is considered very accurate, but we don't have many degrees of freedom using this accurate tool )

Here is the comparison between another simulation tool vs. Pybullet

Anyone knows how can I improve the simulation accuracy?

Here is my code:(xxx represent another simulation tool)

from collections import namedtuple
import pybullet as p
import pybullet_data
import pybullet_envs
import pybullet_robots
import pybullet_utils
from time import sleep
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from attrdict import AttrDict


# Connet to pysical engine
physicsClient = p.connect(p.GUI)  # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())  # used by loadURDF
p.setGravity(0, 0, -10)
# Reset camera
p.resetDebugVisualizerCamera(cameraDistance=3, cameraYaw=90, cameraPitch=-40, cameraTargetPosition=[0, 0, 1])
# Load plane
planeId = p.loadURDF("plane.urdf")
# Load robot
robotStartPos = [0, 0, 0]
robotStartOrn = p.getQuaternionFromEuler([0, 0, 0])
robot = p.loadURDF("urdf/xxxxxx.urdf", robotStartPos, robotStartOrn,
                   flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT, useFixedBase=True)

# Set simulation time
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setRealTimeSimulation(0)
p.setTimeStep(1/240)
# Read speed and pos data from xxx simulation
df_joint_spd = pd.read_csv(r"xxxx\out.speed",
                           delimiter="\s+", header=0)
df_joint_pos = pd.read_csv(r"xxxx\out.pos",
                           delimiter="\s+", header=0)
joint_spd = df_joint_spd.iloc[:, 1:]
joint_pos = df_joint_pos.iloc[:, 1:]

n_spd = joint_spd.shape[0]

# Enable torque sensor
for joint_index in range(6):
    p.enableJointForceTorqueSensor(robot, joint_index, True)


pos_gain = [0.8, 0.8, 0.8, 0.8, 0.8, 0.05]
vel_gain = [1, 1, 1, 1, 1, 1]

bullet_torque = np.zeros([n_spd, 6])
for i in range(n_spd):
    target_spd = joint_spd.iloc[i, :].tolist()
    target_pos = joint_pos.iloc[i, :].tolist()

    p.setJointMotorControlArray(robot, range(6), p.POSITION_CONTROL,
                                targetPositions=target_pos, targetVelocities=target_spd,
                                positionGains=pos_gain, velocityGains=vel_gain
                                )

    # for j in range(6):
    #     p.setJointMotorControl2(robot, j, p.POSITION_CONTROL,
    #                             targetPosition=target_pos[j], targetVelocity=target_spd[j],
    #                             positionGain=pos_gain[j], velocityGain=vel_gain[j]
    #                             )

    p.stepSimulation()
    # read torque information for 6 axis
    for j in range(6):
        jPos, jVel, jForce, jTorque = p.getJointState(robot, j)
        bullet_torque[i, j] = jTorque
    print("Torque : {}".format(bullet_torque[i, :]))
    print("---------------------------------------------------")


# Read Mxperf result
mxperf_torque = pd.read_csv(r"xxxx\out.Tgear",
                            delimiter="\s+", header=0)


plt.figure()
for i in range(6):
    plt.subplot(3, 2, i+1)
    plt.plot(mxperf_torque.iloc[:, i+1], label="another simulation")
    plt.plot(bullet_torque[:, i], label="bullet")
    plt.grid(True)
    plt.legend()
    plt.xlim([5, 170])
    plt.ylim([-joint_list["joint_{}".format(i+1)].maxForce, joint_list["joint_{}".format(i+1)].maxForce])
    plt.title("Axis{}".format(i+1))

plt.show()