[Question] Get torque output / energy consumption of the robot

[yichao-liang]

Hello, as one of the evaluation metric for my experiments, I was wondering if there is a way to get the average torque output by the robot from an episode, or the torque output for every step?

I intend to use this data as an indicator of energy consumption. If you have any alternative metrics or suggestions that could serve this purpose more effectively, I would greatly appreciate your insights.

Thank you for your time and assistance!

[xuanlinli17]

import mani_skill2.envs, gymnasium as gym
import numpy as np

env=gym.make('PickCube-v0')
obs, _ = env.reset()

action = env.action_space.sample()
env.agent.controller.controllers['arm'].set_action(action[:-1])
env.agent.controller.controllers['gripper'].set_action(action[-1:])

joints = env.agent.robot.get_joints()
valid_joint_names = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2']
joints = [x for x in joints if x.name in valid_joint_names]

stiffness = np.array([x.stiffness for x in joints])
damping = np.array([x.damping for x in joints])
force_limit = np.array([x.force_limit for x in joints])

drive_target = env.agent.robot.get_drive_target()
drive_velocity_target = env.agent.robot.get_drive_velocity_target()
cur_qpos = env.agent.robot.get_qpos()
cur_qvel = env.agent.robot.get_qvel()

torque = stiffness * (drive_target - cur_qpos) + damping * (drive_velocity_target - cur_qvel)
torque = np.clip(torque, -force_limit, force_limit)
torque = torque + env.agent.robot.compute_passive_force(True, True, True)
print("torque", torque)

env.step(action)
print(env.agent.robot.get_qpos() - cur_qpos)
print("energy consumption for this control step", np.sum(torque * (env.agent.robot.get_qpos() - cur_qpos)))
# control frequency: env.agent.controller.controllers['arm']._control_freq, env.agent.controller.controllers['gripper']._control_freq, which are 20hz, so power is approximately the above energy consumption * 20