to avoid having to simulate my whole robot assembly and implement inverse kinematics solving I would like to just import the gripper of the robot and set its world pose directly through some API.
What would be the best way to go about this?
Is it preferable to attach the gripper assembly to a kinematics object, whose position is set directly via set_world_pose or should I rather add a 6D joint between the gripper root and the world and set the gripper pose via the dynamic control module?
Would be great if someone could provide some insight and maybe a short code snippet.
As of now I was able to implement a 6D Joint on a simple suction EndEffector and set the desired target pose, by setting the joint targets using the GUI.
How can I set the Joint Targets of a 6D Joint programmatically?
Alternatively I found the “RigidBodyAttractor” in the DynamicControl extension, which (I think) could be used for the same purpose.
I got this working using a simple dynamic cuboid in the scene, however when I use a part of an articulation as the attractor target I get an error message, from the physx C++ file ExtD6Joint.cpp.
2023-05-16 12:20:05 [9,928ms] [Error] [omni.physx.plugin] PhysX error: PxD6JointCreate: actors must be different, FILE /buildAgent/work/16dcef52b68a730f/source/physxextensions/src/ExtD6Joint.cpp, LINE 1141
2023-05-16 12:20:05 [9,928ms] [Error] [omni.isaac.dynamic_control.plugin] Failed to create attractor joint
2023-05-16 12:20:05 [9,928ms] [Error] [omni.isaac.dynamic_control.plugin] DcSetAttractorTarget: Invalid or expired attractor handle
This is the code I used:
import omni
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})
from omni.isaac.core import World
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.dynamic_control import _dynamic_control
from omni.isaac.core.controllers.articulation_controller import ArticulationController
from omni.isaac.core.prims.xform_prim import XFormPrim
import time
world = World()
usd_path = "assets/panda_gripper.usd"
prim_path = "/World/gripper"
prim = XFormPrim(prim_path = prim_path, name="gripper")
add_reference_to_stage(usd_path, prim_path)
world.scene.add(prim)
world.reset()
dc = _dynamic_control.acquire_dynamic_control_interface()
obj_handle = dc.get_rigid_body("/World/gripper/panda_hand")
attractor_properties = _dynamic_control.AttractorProperties()
attractor_properties.body = obj_handle
attractor_properties.axes = _dynamic_control.AXIS_ALL
attractor_properties.target.p = (1, 1, 1)
attractor_properties.target.r = (0, 0, 0, 1)
attractor_properties.stiffness = 1e4
attractor_properties.damping = 1e4
attractor_properties.force_limit = 1e2
attractor = dc.create_rigid_body_attractor(attractor_properties)
while True:
target = _dynamic_control.Transform()
target.p = (0, 0, 0)
dc.set_attractor_target(attractor, target)
for i in range(1000):
world.step()
time.sleep(0.001)
target.p = (1, 1, 1)
dc.set_attractor_target(attractor, target)
for i in range(1000):
world.step()
time.sleep(0.001)
Hi @ManicMunchkin - Apologies for the delay in response. Please let us know if you are still having any issues or questions after using the latest Isaac Sim release.