the command for speed control about UR5e

[zhanglepy]

Intro

Hi!

I am a student at HIT, I use MuJoCo for my research on ur5e control.

My setup

MuJoCo -python、Linux、Ubuntu22.04

My question

Hello everyone, may I ask how to achieve velocity control of the UR5e robotic arm end in Cartesian space in mujoco, such as controlling the speedL instruction in the UR_rtde library for the robotic arm. I know there is position control in the mujoco case, sending POS directly, but I couldn't find the command for speed control.

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.

[HeiMay]

Hi!
I am also working on the end speed control of the ur5 robot. I learned that you can set "actuator" in xml files,like this.
actuator
motor name="torque" joint="joint0" gear="1" ctrllimited="true" ctrlrange="-100 100"/
position name="position_servo" joint="joint0" kp="0"/
velocity name="velocity_servo" joint="joint0" kv="100"/
/actuator

But when I started using the program, no matter how I set the target speed, the robot would always slowly return from the initial position to the position where all the joints were angled 0. This troubled me, and I wondered if it was the Jacobi matrix or something. I've posted my program here in hopes of enlightening you and solving my problems.

This is my program

import numpy as np
import mujoco
import glfw

model = mujoco.MjModel.from_xml_path('/home/mjc/dynamic_insertion/dynamic_insertion/ur5/v/v_control.xml')
data = mujoco.MjData(model)

def init_controller(model, data):

model.actuator_gainprm[:, 0] = 100  
model.actuator_biasprm[:, 1] = -100  

target_end_velocity = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # （x, y, z, rx, ry, rz）

def compute_jacobian(model, data, site_id):
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, site_id)
return np.vstack((jacp, jacr))

def solve_joint_velocities(jacobian, target_end_velocity):
damp = 1e-12
damp_matrix = damp * np.eye(jacobian.shape[1])
jacobian_pinv = np.linalg.pinv(jacobian @ np.linalg.inv(damp_matrix + jacobian.T @ jacobian))
joint_velocities = jacobian_pinv @ target_end_velocity
return joint_velocities

def set_initial_pose(data):
data.qpos[0] = 0.0 # shoulder_pan_joint
data.qpos[1] = -1.5708 # shoulder_lift_joint
data.qpos[2] = 1.5708 # elbow_joint
data.qpos[3] = -1.5708 # wrist_1_joint
data.qpos[4] = -1.5708 # wrist_2_joint
data.qpos[5] = 0.0 # wrist_3_joint
data.qvel[:] = 0.0

def main():
if not glfw.init():
return

window = glfw.create_window(1200, 900, 'UR5 End Effector Control', None, None)
if not window:
    glfw.terminate()
    return

glfw.make_context_current(window)

cam = mujoco.MjvCamera()
opt = mujoco.MjvOption()
scene = mujoco.MjvScene(model, maxgeom=10000)
context = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value)

site_name = 'ee_site'  
site_id = model.site(site_name).id

init_controller(model, data)

set_initial_pose(data)

while not glfw.window_should_close(window):
    jacobian = compute_jacobian(model, data, site_id)

    joint_velocities = solve_joint_velocities(jacobian, target_end_velocity)

    print("Joint velocities:", joint_velocities)

    data.qvel[:model.nu] = joint_velocities[:model.nu]

    mujoco.mj_step(model, data)

    width, height = glfw.get_framebuffer_size(window)
    viewport = mujoco.MjrRect(0, 0, width, height)

    mujoco.mjv_updateScene(model, data, opt, None, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene)
    mujoco.mjr_render(viewport, scene, context)

    glfw.swap_buffers(window)
    glfw.poll_events()

glfw.terminate()

if name == "main":
main()