Assistance Needed with Sequential Movement in Robot Simulation

[nicokl]

Hello,

I’m working on simulating a very basic pick-and-place application using a python extension and a robot that I’ve rigged from a .stp file and for which I’ve developed my own kinematics functions to determine the joint values. The process should simply involve moving the robot to an object, closing the surface gripper, relocating to a different position, and then opening the gripper. While moving to a single position and closing the gripper with a button works as expected, attempting to execute multiple positions sequentially results in the robot bypassing intermediate points and directly moving to the last position. Is there a straightforward method to ensure that each articulation.apply_pose() operation is fully completed before initiating the next move? I’ve tried to figure it out on my own using BaseTask and asyncio, but I can’t seem to get it right.

Any help would be highly appreciated. Thank you!

Best regards,
Nico

[Sirens_uk]

I would check against the current pose as the robot has to move to its target pose which you know as you have defined it in your inverse kinematics solver. so make the current position check against the target position once true,

async def execute(self):
    for pose in self.poses:
        self.articulation.apply_pose(pose)
        await self.wait_until_pose_reached(pose)

async def wait_until_pose_reached(self, target_pose, tolerance=0.01):
# Loop until the current pose matches the target pose within the given tolerance
while not self.pose_reached(target_pose, tolerance):
await asyncio.sleep(0.1) # Check every 100ms

this is just GPT fluff but you get the picture. just match the target postion then do the next articulation in the sequence

[Simplychenable]

here’s an additional simple async snippet in case it’s relevant in your use case:

https://docs.omniverse.nvidia.com/isaacsim/latest/how_to_guides/reference_python_snippets.html#simple-async-task

[nicokl]

Awesome, thank you, those two answers helped a lot!

Here is my solution, for those interested. _apply_pose is called by a ui button.

def _apply_poses(self):
    poses_xyzgr = [[1500, 0, 478, 0, 0], [1850, 0, 2000, 0, 0]]
    poses_angles = [self._get_angles(pose) for pose in poses_xyzgr]

    asyncio.ensure_future(self._execute(poses_angles))
    
def _get_angles(self, pose_xyzgr):
    t0, t1, t2, t3 = self.kuka.get_joint_angles(
        pose_xyzgr[0], 
        pose_xyzgr[1],
        pose_xyzgr[2],
        pose_xyzgr[3]
              )
    t4 = math.radians(pose_xyzgr[4])
    
    return np.array([t0, -t1, t2, -t3, t4]) 

async def _execute(self, poses_angles):
    for pose in poses_angles:
        robot_action = ArticulationAction(joint_positions = pose)
        self.articulation.apply_action(robot_action)
        await self._wait_until_pose_reached(pose)

async def _wait_until_pose_reached(self, pose):
    while not self._pose_reached(pose, 0.01):
        await asyncio.sleep(0.1) 
    return True

def _pose_reached(self, target_pose, tolerance): 
    for val1, val2 in zip(target_pose, self.articulation.get_joint_positions()):
        if abs(val1 - val2) > tolerance:
            return False
    return True