# 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 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.robots import Robot
from omni.isaac.dynamic_control import _dynamic_control
from omni.isaac.core import SimulationContext
import numpy as np
import carb

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)
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"
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")
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:
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:
return

def get_observations(self):
observations = {
}
return observations

def post_reset(self):
return

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

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"
return

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.
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()
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)