Torque control implementation

Hi all
I am trying to implement a simple torque control for a Franka robot in Isaac Sim. I am doing this by modifying franka_articulation.py.
It looks like dc.set_dof_position_target works fine and sends the robot to the desired position. However, I dont see any motions when calling dc.set_dof_velocity_target or dc.apply_dof_effort to control the joint velocity or torque, although both of these calls return true, i.e. success.

Are there any resources/examples that can help clarifying these use cases?

Any help will be appreciated.

Thanks,
HA

Here is the code:

import os
import carb
from omni.isaac.python_app import OmniKitHelper
import time
import math
import numpy as np
CONFIG = {
“experience”: f’{os.environ[“EXP_PATH”]}/omni.isaac.sim.python.kit’,
“renderer”: “RayTracedLighting”,
“headless”: False,
}
if name == “main”:
# Example usage, with step size test
kit = OmniKitHelper(config=CONFIG)
import omni.physx
from omni.isaac.dynamic_control import _dynamic_control
from omni.isaac.utils.scripts.nucleus_utils import find_nucleus_server
stage = kit.get_stage()
asset_path="/home/user/isaac_models/franka1.usd"
omni.usd.get_context().open_stage(asset_path)
kit.play()
# # perform step experiments
kit.update(1.0 / 10.0,1.0/500.0,50)
dc = _dynamic_control.acquire_dynamic_control_interface()
ar = dc.get_articulation("/Franka")
if ar == _dynamic_control.INVALID_HANDLE:
print("*** ‘%s’ is not an articulation" % “/Franka”)
else:
root = dc.get_articulation_root_body(ar)
print(str(“Got articulation handle %d \n” % ar) + str("— Hierarchy\n"))
body_states = dc.get_articulation_body_states(ar, _dynamic_control.STATE_ALL)
print(str("— Body states:\n") + str(body_states) + “\n”)
dof_states = dc.get_articulation_dof_states(ar, _dynamic_control.STATE_ALL)
print(str("— DOF states:\n") + str(dof_states) + “\n”)
dof_props = dc.get_articulation_dof_properties(ar)
print(str("— DOF properties:\n") + str(dof_props) + “\n”)
dof_ptr=[dc.find_articulation_dof(ar,“panda_joint”+str(i)) for i in range(1,8)]
q=[dc.get_dof_position(dof_ptr[i]) for i in range(7)]
qd=q.copy()
qd[1]=-1.5
for i in range(1000):
kit.update(1.0 / 10.0,1.0/500.0,50)
dc.wake_up_articulation(ar)
q=[dc.get_dof_position(dof_ptr[i]) for i in range(7)]
e=list(np.asarray(qd)-np.asarray(q))
k=10
tau_d=[k*e[i] for i in range(7)]
res=[dc.apply_dof_effort(dof_ptr[i],tau_d[i]) for i in range(7)]
print(res,tau_d)
# cleanup
kit.stop()
kit.shutdown()

Thanks for the question @h.azimian will get back to you with a sample showing how to use position/velocity control. torque control should work, but I need to confirm.

Hi @h.azimian , thanks for the question. Regarding the torque control you can use dc.apply_dof_effort or dc.apply_articulation_dof_efforts as you stated, the reason you didn’t see any motion is because of the scale, the USD of the franka arm has a stage scale of CM (we are in the process of changing this for the next release). So you will have to apply a larger magnitude to see any motion. You can use the following script in the script editor window after creating a franka arm and starting the simulation.

from omni.isaac.dynamic_control import _dynamic_control
import numpy as np
dc = _dynamic_control.acquire_dynamic_control_interface()
art = dc.get_articulation("/Franka")
dc.wake_up_articulation(art)
joint_efforts = [-np.random.rand(9) * 1000 * (100**2)]
dc.apply_articulation_dof_efforts(art, joint_efforts)

Regarding velocity control, there are several ways to achieve this. We will be adding some samples in the next release. For now you can use the following script to do so. Please note that you need to change the drive stiffness to zero for the corresponding joints before the articulation gets registered in dynamic_control so before this line specifically - art = dc.get_articulation("/Franka") -

from pxr import UsdPhysics
stage = omni.usd.get_context().get_stage()
for prim in stage.TraverseAll():
  prim_type = prim.GetTypeName()
  if prim_type in [“PhysicsRevoluteJoint” , “PhysicsPrismaticJoint”]:
    if prim_type == “PhysicsRevoluteJoint”:
      drive = UsdPhysics.DriveAPI.Get(prim, “angular”)
    else:
      drive = UsdPhysics.DriveAPI.Get(prim, “linear”)
    drive.GetStiffnessAttr().Set(0)
from omni.isaac.dynamic_control import _dynamic_control
import numpy as np
dc = _dynamic_control.acquire_dynamic_control_interface()
art = dc.get_articulation("/Franka")
dc.wake_up_articulation(art)
joint_vels = [-np.random.rand(9)*10]
dc.set_articulation_dof_velocity_targets(art, joint_vels)

You can also see the attached videos for a step by step illustration through the UI, likewise you can use the same snippets of code if you are running IsaacSim headless and launching it from python. Please let us know if you have any other questions.