Manipulator control problems for mobile manipulators

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
Below is the code used in my video. (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__(
        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
    def set_up_scene(self, 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")
    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
    def get_observations(self):
        observations = {
        return observations
    def post_reset(self):
        self._task_event = 0

class HelloWorld(BaseSample):
    def __init__(self) -> None:

    def setup_scene(self):
        world = self.get_world()
        # 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"))

    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 = np.array([5.0,5.0,5.0,5.0], dtype=np.float32)
        self.previous_error = np.zeros_like(
        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)

    def control(self,current, dt, Kp=0.005, Ki=0.005, Kd=0):

        error = - 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]
        effort = self.control(self.dof_states, step_size)
        effort = effort.astype(np.float32)
        self.dci.set_articulation_dof_efforts(self.art_car, 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.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.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)

thank you for your help. @rgasoto

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.