I am trying to move a robot from one pose to another pose using an articulation control.
But the robot is not moving to the final pose from the initial pose it is constantly moving to another different pose…
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": True})
from omni.isaac.core import World
from omni.isaac.core.robots import Robot
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.utils.stage import add_reference_to_stage, get_stage_units
from omni.isaac.core.controllers import ArticulationController
import numpy as np
import sys
initial_posture = np.array([-3.4930844,-1.7593288, -1.2764046, -1.676637, 1.5707291, -1.9221054])
final_posture = np.array([1.0528518, -1.8536792, -0.02006058, -0.7816168, 1.2877516, 1.8681929])
vel = np.array([0.5,0.5,0.5,0.5,0.5,0.5])
my_world = World(stage_units_in_meters=1.0)
my_world.scene.add_default_ground_plane()
asset_path = "/home/bikram/ur3.usd"
add_reference_to_stage(usd_path=asset_path, prim_path="/World/UR3_1")
my_ur = my_world.scene.add(Robot(prim_path="/World/UR3_1", name="my_ur"))
my_controller = ArticulationController()
my_world.reset()
my_ur.initialize()
for i in range(10):
if my_world.is_playing():
if my_world.current_time_step_index == 0:
my_world.reset()
my_world.reset()
my_ur.set_world_pose(position=np.array([0.0, 0.0, 0.0]) / get_stage_units())
my_ur.set_joint_positions(initial_posture)
for j in range(70000):
my_world.step(render = True)
#print("action command now.......")
my_ur.get_articulation_controller().apply_action(
ArticulationAction(joint_positions = final_posture, joint_velocities = final_posture)
)
if (np.array(my_ur.get_joint_positions()) == final_posture).all() == True:
print("loop broken..")
break
simulation_app.close()
The final pose in this video is not the final pose that I have passed through the ArticulationAction.
Please check and correct the code if anyone can. Thanks in advance.