Trouble controlling robotiq gripper using Isaac Lab
Hey guys,
I created a custom USD with robotiq gripper and a couple of prismatic joints to be able to move around the gripper once the object is grasped. When I set the velocity target to “finger joint” and “right_outer_knuckle_joint” in the Isaac Sim UI, I can see that the gripper is able to grasp the object.
But when I import the USD into my Isaac Lab interactive scene cfg and try to set the velocity target using “set_joint_velocity_target”, the joints move even after the fingers make contact with the object. Has anybody seen this before? Any pointers is welcome. I am unable to find documentation for this.
USD file is in the attached zip and below is the code. Do you see any obvious point that I may be missing here?
import argparse
from isaaclab.app import AppLauncher
# create argparser
parser = argparse.ArgumentParser(description="Tutorial on spawning prims into the scene.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
"""Rest everything follows."""
import numpy as np
import torch
import time
import isaacsim
import isaacsim.core.utils.prims as prim_utils
from isaacsim.robot.manipulators.grippers import ParallelGripper
import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.sim import SimulationContext
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.managers import SceneEntityCfg
from isaacsim.core.utils.types import ArticulationAction
import isaaclab.envs.mdp as mdp
@configclass
class RobotiqGripper(InteractiveSceneCfg):
# ground plane
ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg(), init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -0.1]))
# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)
# Add Robotiq gripper articulation
gripper = ArticulationCfg(
prim_path="/World/Gripper",
spawn=sim_utils.UsdFileCfg(
usd_path="<insert_usd_path_here",
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_depenetration_velocity=5,
),
),
init_state = ArticulationCfg.InitialStateCfg(pos = [0, 0, 0]),
actuators={
"finger_joint": ImplicitActuatorCfg(
joint_names_expr=["finger_joint"],
velocity_limit=80,
effort_limit=2,
stiffness=0.0,
damping=5000.0,
),
"right_outer_knuckle_joint": ImplicitActuatorCfg(
joint_names_expr=["right_outer_knuckle_joint"],
velocity_limit=80,
effort_limit=2,
stiffness=0.0,
damping=5000.0,
),
"left_outer_finger_joint": ImplicitActuatorCfg(
joint_names_expr=["left_outer_finger_joint"],
velocity_limit=0,
effort_limit=0,
stiffness=0.05,
damping=0.0,
),
"right_outer_finger_joint": ImplicitActuatorCfg(
joint_names_expr=["right_outer_finger_joint"],
velocity_limit=0,
effort_limit=0,
stiffness=0.05,
damping=0.0,
),
"left_inner_finger_joint": ImplicitActuatorCfg(
joint_names_expr=["left_inner_finger_joint"],
velocity_limit=0,
effort_limit=0,
stiffness=0.0,
damping=0.0,
),
"right_inner_finger_joint": ImplicitActuatorCfg(
joint_names_expr=["right_inner_finger_joint"],
velocity_limit=0,
effort_limit=0,
stiffness=0.0,
damping=0.0,
),
"left_inner_finger_knuckle_joint": ImplicitActuatorCfg(
joint_names_expr=["left_inner_finger_knuckle_joint"],
velocity_limit=0,
effort_limit=0,
stiffness=0.0,
damping=0.0,
),
"right_inner_finger_knuckle_joint": ImplicitActuatorCfg(
joint_names_expr=["right_inner_finger_knuckle_joint"],
velocity_limit=0,
effort_limit=0,
stiffness=0.0,
damping=0.0,
),
},
)
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
robot = scene["gripper"]
print(robot.data.joint_names)
count = 0
while simulation_app.is_running():
if count % 500 == 0:
# reset counter
count = 0
root_state = robot.data.default_root_state.clone()
root_state[:, :3] += scene.env_origins
robot.write_root_pose_to_sim(root_state[:, :7])
robot.write_root_velocity_to_sim(root_state[:, 7:])
# set joint positions with some noise
joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone()
joint_pos += torch.zeros_like(joint_pos)
robot.write_joint_state_to_sim(joint_pos, joint_vel)
# clear internal buffers
scene.reset()
print("[INFO]: Resetting robot state...")
target_velocity = torch.zeros(1, 10).to(robot.device)
target_velocity[0, 2:4] = 5
robot.set_joint_velocity_target(target_velocity)
robot.write_data_to_sim()
count += 1
sim.step()
def main():
"""Main function."""
# Load kit helper
sim_cfg = sim_utils.SimulationCfg()
sim_cfg.dt = 0.005
sim = SimulationContext(sim_cfg)
sim.set_camera_view([2.5, 0.0, 2.5], [0.0, 0.0, 1.0])
# Design scene
scene_config = RobotiqGripper(num_envs=1, env_spacing=2.0)
scene = InteractiveScene(scene_config)
sim.reset()
print("[INFO]: Setup complete...")
# Run the simulator
run_simulator(sim, scene)
if __name__ == "__main__":
# run the main function
main()
# close sim app
simulation_app.close()
barebone_test.zip (5.6 KB)