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.
This tutorial is nice!
The only thing I didnt get is: I have to replace dc.apply_articulation_dof_efforts(art, joint_efforts) to dc.set_articulation_dof_efforts(art, joint_efforts)
Is my usd version different or do I need to set something?
And in addition, I tried to grasp deformable objects with Franka “torque-controlled” gripper.
the displacement seems not correct with the object, there is a gap between deformable object and the gripper fingers.
How does joint controlled? (Franka Gripper)
I tried to set Franka Gripper joint position target and velocity target at the same time, the gripper moves.
However, I use dc.get_dof_effort(gripper_joint) it returns 0, is there no torque applied to control the joint?
dc.set_dof_effort(joint, effort) seems not actually applying the commanded torque value.
It seems the effort is calculated based on the target position X and velocity V. Effort= K_joint * (X_now- X) + D * (V_now - V). The equation is correct, but the joint seems to be not in torque control.
I have a desired joint position and velocity, I use the equation Effort= K_joint * (X_now- X) + D * (V_now - V) to get my desired effort and do dc.set_dof_effort(joint, effort), but since the target position and velocity is predefined in USD file, I can not control the real desired joint position and velocity.
What I want to implement is a simple PD controller for joint, which out puts desired torque to actuate the joint to get desired position and velocity.
I can set desired position and velocity with dc.set_dof_position_target() and dc.set_dof_velocity_target(), the robot moves but when I call dc.get_dof_effort(gripper_joint) it returns 0. It seems there is no torque that actually controlling this motion.
And also, I want to add a contact sensor for finger tip, it also does not work
Is torque (effort) control a conflicting matter with velocity or position control?
If I want to write a PD controller by myself, and to achieve pure torque control to joints, each parameter like K and D, how to set them? It is recommended in doc, in position control you have to set K and D, and in velocity control you set K lower
What I have tried for question 1 is that, I set K=0, i.e. a pure damping control. And I calculate desired torque with my PD controller. But this makes no sense, since the idea of PD control is to design your desired motion impedance, there will be two dynamics coupling each other
I conducted an experience and confirmed the torque can’t be applied to the joint via set_dof_torque(...) or set_dof_state(...). However, the position and velocity can be set by set_dof_state(...), no matter if the Angular Drive is mounted. If the Angular Drive is mounted, the position and velocity will be overwritten.
And I dont understand how the joint is drived exactly.
get_dof_effort only returns effort = K_usd * (set_dof_position_target - init_position_usd) + D_usd * (set_dof_velocity_target - init_velocity_usd), where init_position_usd and init_velocity_usd is defined as joint parameters in usd file. This is not the real torque the joint should output.
Driving torque should be K_usd * (set_dof_position_target - position_now) + D_usd * (set_dof_velocity_target - velocity_now)
This would be tricky for applications involving contacts.
If you set the drive stiffness and damping to zero you should be able to do torque control. This way the position/velocity target do night fight the specified torque value.
Hello, may i ask you some question?
Where did you find effort = K_usd * (set_dof_position_target - position_now ) + D_usd * (set_dof_velocity_target - velocity_now )? I just find this :stiffness * (position - target_position) + damping * (velocity - target_velocity) in create docs which is a resistance against motion. And when i use urdf files to import my own robot, i can also use set_dof_effort to control my robot. So i think maybe there is no target_pos and target_vel in usd files?
And i am trying to construct kinetic model of a wheel(such as T - Tload = Jdw/dt), i dont know how to make it.because i dont know how does friction, damping etc. work. maybe you have some advise?
