orbit
orbit copied to clipboard
[Question] Contact Sensor on UR10 ee_link does not work
Hi, I'm currently work with the contact sensor.
The example (source/standalone/tutorials/04_sensors/add_sensors_on_robot.py) works well in my environment.
To test with my configuration, I've attached a contact sensor on my UR10 ee_link.
Then I planned the robot to gently push an object on an table.
However, the sensor always returns (net_forces_w=tensor([[[0., 0., 0.]]]
)
Some essential code for my environment is as below:
@configclass
class URSceneCfg(InteractiveSceneCfg):
target_obj = AssetBaseCfg(
prim_path="{ENV_REGEX_NS}/object",
spawn=sim_utils.UsdFileCfg(usd_path="omniverse://localhost/NVIDIA/Assets/Isaac/4.0/Isaac/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd"),
init_state=AssetBaseCfg.InitialStateCfg(pos=(0.8, 0.02, 0.1), rot=(1, 0.0, 0.0, 0.0)))
# ur10 robot
robot: ArticulationCfg = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
contact_forces = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot/ee_link",
update_period=0.0,
history_length=6,
debug_vis=True,
track_pose=True,
)
...
## simulation loop
while simulation_app.is_running():
if ALL_ENV_FIN == True or count == 0:
## reset the simulation
print("All environments finished!")
print("Resetting the simulation...")
if ALL_ENV_FIN:
count = 0
ALL_ENV_FIN = False
root_state = scene["robot"].data.default_root_state.clone()
root_state[:, :3] += scene.env_origins
scene["robot"].write_root_state_to_sim(root_state)
## reset joint state
joint_pos = ur10.data.default_joint_pos.clone()
joint_vel = ur10.data.default_joint_vel.clone()
ur10.write_joint_state_to_sim(joint_pos, joint_vel)
ur10.reset()
## reset actions
ik_commands[:] = ee_goals[current_goal_idx]
joint_pos_des = joint_pos[:, robot_entity_cfg.joint_ids].clone()
## reset controller
diff_ik_controller.reset()
diff_ik_controller.set_command(ik_commands)
if ALL_ENV_ARRV == True:
## robot has arrived to the home position and inference is ready!
if current_goal_idx == 0:
print("All environments arrived!")
current_goal_idx = (current_goal_idx + 1) % len(ee_goals)
# reset actions
ik_commands[:] = ee_goals[current_goal_idx]
# reset controller
diff_ik_controller.reset()
diff_ik_controller.set_command(ik_commands)
ALL_ENV_ARRV = False
else:
# obtain quantities from simulation
jacobian = ur10.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
ee_pose_w = ur10.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
root_pose_w = ur10.data.root_state_w[:, 0:7]
joint_pos = ur10.data.joint_pos[:, robot_entity_cfg.joint_ids]
## compute frame in root frame
## ee_pose_w[:, 0:3] -> position of robots based on world frame (considering the spacing)
## ee_pos_b = position of each robot based on each robot's frame
ee_pos_b, ee_quat_b = subtract_frame_transforms(
root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
)
## compute the joint commands
joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
## Developing
if torch.sum(torch.norm(ee_pos_b - ik_commands[0][:3], dim=1) < 0.03) >= args_cli.num_envs:
arrv_count += 1
print("Goal reached!")
if arrv_count == 20:
ALL_ENV_ARRV = True
arrv_count = 0
## Apply action
## -- apply action to the robot
ur10.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
## -- write data to sim
scene.write_data_to_sim()
## Perform step
sim.step()
## Increment counter
count += 1
## update buffers
scene.update(sim_dt)
## obtain quantities from simulation
## update marker positions
# ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
print("-------------------------------")
print(ur10.data.body_pos_w)
print(ur10.body_names)
print(scene['contact_forces'].data.pos_w)
print()
print("contact force: ", scene["contact_forces"].data)
print("contact sensor initialized: ", scene["contact_forces"].is_initialized)
Thank you!