Hello, I am doing a simulation experiment. In the experiment, I let the manipulator move between two fixed postures, but now I want to speed up the movement of the manipulator. I read some documents and only saw the setting of the target joint speed function, but it seems to affect the target position of the robot arm I set before. How can I speed up the movement of the robot arm from one pose to another? Thank you for your help.
Can you explain in more detail how are you controlling the manipulator? some code snippets and videos would be greatly appreciated
ok .here is my video link https://youtu.be/LXXYHJEgOUM.
Below is the code used in my video.
hello_world_extension.py (777 Bytes)
I want to speed up the movement of the manipulator in the video at the two target positions I set.But, I’m sorry I don’t know what to do. I would be very grateful if you could help me. @rgasoto
It’s still not clear how you are setting the manipulator poses. The code seems to be empty, besides starting the extension itself. were you supposed to send a different file?
On the video I see you set the wheels damping and stiffness to zero, which allows the moving base to react to the arm movement.
When it comes to the arm manipulation, how do you set the target pose and velocity?
Bear in mind that the joint drive is a dual-target PD controller - It tracks position and velocity simultaneously by the formula
joint effort = stiffness*(position－target_position)+damping*(velocity－target_velocity)
To make it a perfect PD controller for position, set the target velocity to zero, and the velocity tracking will try to keep the robot immobile. Changing the velocity target when you have some stiffness value will cause the robot to move ever so slightly until the position error dominates the equation and then the robot will stop near the position target, with a slight offset given by the target_velocity above zero.
Sorry, it was my reason that gave the wrong code file
This is the correct code:
from omni.isaac.examples.base_sample import BaseSample
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.core.tasks import BaseTask
from omni.isaac.dynamic_control import _dynamic_control
from omni.isaac.core import SimulationContext
import numpy as np
import carb
class RobotsPlaying(BaseTask):
def __init__(
self,
name
):
super().__init__(name=name, offset=None)
self._joint_pos1 = np.array([1.57, 0, 0, -1.57, 0, 1.57, 0, 0, 0], dtype=np.float32)
self._joint_pos2 = np.array([1.57, 0, 1.57, -1.57, 0, 1.57, 0, 0, 0], dtype=np.float32)
self._task_event = 0
return
def set_up_scene(self, scene):
super().set_up_scene(scene)
assets_root_path = get_assets_root_path()
asset_path = "/home/ylc/G/lula.usd"
add_reference_to_stage(usd_path=asset_path, prim_path="/World/Fancy_Robot")
return
def pre_step(self, control_index, simulation_time):
dci = _dynamic_control.acquire_dynamic_control_interface()
art_arm = dci.get_articulation("/World/Fancy_Robot/franka")
if self._task_event == 0:
dof_state = dci.get_articulation_dof_states(art_arm, 1)
joint_states = [row[0] for row in dof_state]
if np.mean(np.abs(joint_states - self._joint_pos1)) < 0.04:
self._task_event += 1
elif self._task_event == 1:
dof_state = dci.get_articulation_dof_states(art_arm, 1)
joint_states = [row[0] for row in dof_state]
if np.mean(np.abs(joint_states - self._joint_pos2)) < 0.04:
self._task_event -= 1
return
def get_observations(self):
observations = {
"task_event":self._task_event
}
return observations
def post_reset(self):
self._task_event = 0
return
class HelloWorld(BaseSample):
def __init__(self) -> None:
super().__init__()
return
def setup_scene(self):
world = self.get_world()
world.scene.add_default_ground_plane()
world.add_task(RobotsPlaying(name="awesome_task"))
# if assets_root_path is None:
# carb.log_error("Could not find nucleus server with /Isaac folder")
#asset_path = assets_root_path + "/Isaac/Robots/Jetbot/jetbot.usd"
# jackal_robot = world.scene.add(Robot(prim_path="/World/Fancy_Robot", name="jackal_basic"))
return
async def setup_post_load(self):
self._world = self.get_world()
# self._jackal = self._world.scene.get_object("jackal_basic")
# This is an implicit PD controller of the jetbot/ articulation
# setting PD gains, applying actions, switching control modes..etc.
# can be done through this controller.
# Note: should be only called after the first reset happens to the world
# self._jackal_articulation_controller = self._jackal.get_articulation_controller()
dci = _dynamic_control.acquire_dynamic_control_interface()
self.dci = dci
art_car = dci.get_articulation("/World/Fancy_Robot/jackal_basic")
art_arm = dci.get_articulation("/World/Fancy_Robot/franka")
self.art_car = art_car
self.art_arm = art_arm
# print position for the degree of freedom
self.target = np.array([5.0,5.0,5.0,5.0], dtype=np.float32)
self.previous_error = np.zeros_like(self.target)
self.integral = 0
# Adding a physics callback to send the actions to apply actions with every
# physics step executed.
self._world.add_physics_callback("sending_actions", callback_fn=self.step_callback)
return
def control(self,current, dt, Kp=0.005, Ki=0.005, Kd=0):
error = self.target - current
self.integral += error * dt
derivative = (error - self.previous_error) / dt
self.previous_error = error
return Kp * error + Ki * self.integral + Kd * derivative
def step_callback(self,step_size):
# print("simulate with step: ", step_size)
dof_fl = self.dci.find_articulation_dof(self.art_car, "front_left_wheel")
dof_fr = self.dci.find_articulation_dof(self.art_car, "front_right_wheel")
dof_rl = self.dci.find_articulation_dof(self.art_car, "rear_left_wheel")
dof_rr = self.dci.find_articulation_dof(self.art_car, "rear_right_wheel")
joint_effort = [0.01,0.1,0.01,0.1]
dof_state_fl = self.dci.get_dof_state(dof_fl,7)
dof_state_fr = self.dci.get_dof_state(dof_fr,7)
dof_state_rl = self.dci.get_dof_state(dof_rl,7)
dof_state_rr = self.dci.get_dof_state(dof_rr,7)
# print position for the degree of freedom
self.dof_states = [dof_state_fl.vel,dof_state_fr.vel,dof_state_rl.vel,dof_state_rr.vel]
print(self.dof_states)
effort = self.control(self.dof_states, step_size)
effort = effort.astype(np.float32)
self.dci.wake_up_articulation(self.art_car)
self.dci.set_articulation_dof_efforts(self.art_car, effort)
print(effort)
current_observations = self._world.get_observations()
if current_observations["task_event"] == 0:
joint_pos1 = np.array([1.57, 0, 0, -1.57, 0, 1.57, 0, 0, 0], dtype=np.float32)
self.dci.wake_up_articulation(self.art_arm)
self.dci.set_articulation_dof_position_targets(self.art_arm, joint_pos1)
elif current_observations["task_event"] == 1:
joint_pos2 = np.array([1.57, 0, 1.57, -1.57, 0, 1.57, 0, 0, 0], dtype=np.float32)
self.dci.wake_up_articulation(self.art_arm)
self.dci.set_articulation_dof_position_targets(self.art_arm, joint_pos2)
# print(current_observations["task_event"])
dof_state_vel = self.dci.get_articulation_dof_states(self.art_arm, 2)
joint_state_vel = [row[1] for row in dof_state_vel]
# print(joint_state_vel)
return
thank you for your help. @rgasoto
This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.