Getting error when using the codebase for XArm 7
shivanshpatel35 opened this issue · comments
Hi,
I am trying to use your codebase to control XArm robot. Its XML is here. When I run diffik_nullspace.py after changing the path to XArm7 here, I get the error here.
operands could not be broadcast together with shapes (13,) (7,)
There are few discrepancies that I noticed:
- jac = np.zeros((6, model.nv)) here. My model.nv is 13. I guess it should be rather 7 (== degree of freedom).
- q0 = model.key(key_name).qpos here. q0's shape is (13,) even though I feel it should be (7,).
Am I interpreting anything wrong?
Are you using the xarm with the gripper? If so, you need to mask out the dofs corresponding to the gripper and make sure you index correctly into the different matrices. I’ll let you give it a go, if you get stuck, come back and let me know and I can give some pointers.
Just indexing the matrices here made it work for XArm7 with gripper.
Thanks a lot, Kevin!
Hi @shivanshpatel35 and @kevinzakka !
I'm running into a similar issue, though in my case, i've added a red_cube box geom to my scene.xml with a free joint which is causing some breaking changes to the diffik_nullspace.py code. I tried indexing the matrices, but indexing didn't work for me : /
The issue occurs when doing mujoco.mj_integratePos(model, q, dq, integration_dt) (see code below).
Please let me know if you have any pointers
The minimal XML I use for the task is provided:
<mujoco model="panda scene">
<include file="panda_nohand.xml"/>
<statistic center="0.3 0 0.35" extent="0.8" meansize="0.05"/>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
<global azimuth="120" elevation="-20"/>
</visual>
<asset>
<texture name="grid" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512"
mark="cross" markrgb=".8 .8 .8"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true"/>
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom name="floor" size="1 1 0.01" type="plane" material="grid"/>
<body name="target" pos="0.5 0 .5" quat="0 1 0 0" mocap="true">
<geom type="box" size=".05 .05 .05" contype="0" conaffinity="0" rgba=".6 .3 .3 .5"/>
<site type="sphere" size="0.01" rgba="0 0 1 1" group="1"/>
</body>
<body name="red_cube">
<freejoint/>
<geom name="red_cube" type="box" size="0.05 0.05 0.05" rgba="1 0 0 1"/>
</body>
</worldbody>
<!-- Panda Arm: 0 0 0 -1.57079 0 1.57079 -0.7853 -->
<!-- Red Cube: 0.3 0 0 1 0 0 0 -->
<keyframe>
<key name="home" qpos="0 0 0 -1.57079 0 1.57079 -0.7853 0.3 0 0 1 0 0 0" ctrl="0 0 0 -1.57079 0 1.57079 -0.7853"/>
</keyframe>
</mujoco>The diffik_nullspace.py file i currently have is:
import mujoco
import mujoco.viewer
import numpy as np
import time
import os
# Integration timestep in seconds. This corresponds to the amount of time the joint
# velocities will be integrated for to obtain the desired joint positions.
integration_dt: float = 0.1
# Damping term for the pseudoinverse. This is used to prevent joint velocities from
# becoming too large when the Jacobian is close to singular.
damping: float = 1e-4
# Gains for the twist computation. These should be between 0 and 1. 0 means no
# movement, 1 means move the end-effector to the target in one integration step.
Kpos: float = 0.95
Kori: float = 0.95
# Whether to enable gravity compensation.
gravity_compensation: bool = True
# Simulation timestep in seconds.
dt: float = 0.002
# Nullspace P gain.
Kn = np.asarray([10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0])
# Maximum allowable joint velocity in rad/s.
max_angvel = 0.785
def main() -> None:
assert mujoco.__version__ >= "3.1.0", "Please upgrade to mujoco 3.1.0 or later."
# Load the model and data.
model_path = os.path.join(os.path.dirname(__file__), "franka_emika_panda/scene.xml")
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)
# Enable gravity compensation. Set to 0.0 to disable.
model.body_gravcomp[:] = float(gravity_compensation)
model.opt.timestep = dt
# End-effector site we wish to control.
site_name = "attachment_site"
site_id = model.site(site_name).id
# Get the dof and actuator ids for the joints we wish to control. These are copied
# from the XML file. Feel free to comment out some joints to see the effect on
# the controller.
joint_names = [
"joint1",
"joint2",
"joint3",
"joint4",
"joint5",
"joint6",
"joint7",
]
dof_ids = np.array([model.joint(name).id for name in joint_names])
actuator_ids = np.array([model.actuator(name).id for name in joint_names])
# Initial joint configuration saved as a keyframe in the XML file.
key_name = "home"
key_id = model.key(key_name).id
q0 = model.key(key_name).qpos
# Mocap body we will control with our mouse.
mocap_name = "target"
mocap_id = model.body(mocap_name).mocapid[0]
# Pre-allocate numpy arrays.
jac = np.zeros((6, model.nv))
diag = damping * np.eye(6)
eye = np.eye(model.nv)
twist = np.zeros(6)
site_quat = np.zeros(4)
site_quat_conj = np.zeros(4)
error_quat = np.zeros(4)
with mujoco.viewer.launch_passive(
model=model,
data=data,
show_left_ui=False,
show_right_ui=False,
) as viewer:
# Reset the simulation.
mujoco.mj_resetDataKeyframe(model, data, key_id)
# Reset the free camera.
mujoco.mjv_defaultFreeCamera(model, viewer.cam)
# Enable site frame visualization.
viewer.opt.frame = mujoco.mjtFrame.mjFRAME_SITE
while viewer.is_running():
step_start = time.time()
# Spatial velocity (aka twist).
dx = data.mocap_pos[mocap_id] - data.site(site_id).xpos
twist[:3] = Kpos * dx / integration_dt
mujoco.mju_mat2Quat(site_quat, data.site(site_id).xmat)
mujoco.mju_negQuat(site_quat_conj, site_quat)
mujoco.mju_mulQuat(error_quat, data.mocap_quat[mocap_id], site_quat_conj)
mujoco.mju_quat2Vel(twist[3:], error_quat, 1.0)
twist[3:] *= Kori / integration_dt
# Jacobian.
mujoco.mj_jacSite(model, data, jac[:3], jac[3:], site_id)
# Damped least squares.
dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist)
# Nullspace control biasing joint velocities towards the home configuration.
# I broke down the computation of the nullspace control into Left and Right hand side
# I checked the shapes to ensure that the shapes matched
lhs = eye - np.linalg.pinv(jac) @ jac
lhs = lhs[:7, :7]
print("lhs", lhs.shape)
rhs = Kn * (q0[:7] - data.qpos[dof_ids])
print("rhs", rhs.shape)
dq[:7] += lhs @ rhs # After doing lhs @ rhs, the shape of the result is (7,) which is the same as dq[:7]
# Clamp maximum joint velocity.
dq_abs_max = np.abs(dq).max()
if dq_abs_max > max_angvel:
dq *= max_angvel / dq_abs_max
# Integrate joint velocities to obtain joint positions.
# I only use the first 13 values, so shape of q matches the shape of dq (13,)
q = data.qpos.copy()[:13] # Note the copy here is important.
mujoco.mj_integratePos(model, q, dq, integration_dt) # bug occurs here
np.clip(q, *model.jnt_range.T, out=q)
# Set the control signal and step the simulation.
data.ctrl[actuator_ids] = q[dof_ids]
mujoco.mj_step(model, data)
viewer.sync()
time_until_next_step = dt - (time.time() - step_start)
if time_until_next_step > 0:
time.sleep(time_until_next_step)
if __name__ == "__main__":
main()