Question about sending motion command with uncalibrated URDF

[Dipan-Zhang]

Hi all,

When I'm reading the code about sending motion commands to the robot, I found out that the common practice is to compute IK with an uncalibrated URDF at the server side.

And I've tested that an uncalibrated URDF can have a motion error of ~ 15mm, which significantly affects the motion accuracy. I'm wondering if there is a specific reason for computing FK/IK with an uncalibrated URDF within the server?

An Example here, use the robot model to solve IK and get joint positions,

stretch_ai/src/stretch/agent/operations/grasp_object.py

Lines 1075 to 1096 in eb6f57d

	def grasp_open_loop(self, object_xyz: np.ndarray):
	"""Grasp the object in an open loop manner. We will just move to object_xyz and close the gripper.
	Args:
	object_xyz (np.ndarray): Location to grasp
	Returns:
	bool: True if successful, False otherwise
	"""
	model = self.robot.get_robot_model()
	xyt = self.robot.get_base_pose()
	relative_object_xyz = point_global_to_base(object_xyz, xyt)
	joint_state = self.robot.get_joint_positions()
	# We assume the current end-effector orientation is the correct one, going into this
	ee_pos, ee_rot = model.manip_fk(joint_state)
	# If we failed, or if we are not servoing, then just move to the object
	target_joint_positions, _, _, success, _ = self.robot_model.manip_ik_for_grasp_frame(
	relative_object_xyz, ee_rot, q0=joint_state
	)

Robot kinematic model initiated from uncalibrated URDF (by default):

stretch_ai/src/stretch/motion/kinematics.py

Lines 192 to 197 in eb6f57d

	# urdf
	if not urdf_path:
	manip_urdf = MANIP_STRETCH_URDF
	else:
	manip_urdf = os.path.join(urdf_path, "stretch.urdf")
	self.manip_mode_urdf_path = os.path.join(root, manip_urdf)

I found out there exists a better solution to directly call goto_ee_pose (which already exists in manip.py) and solve the Ik from the robot side with calibrated URDF

stretch_ai/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/manip.py

Lines 305 to 342 in eb6f57d

	@enforce_enabled
	def goto_ee_pose(
	self,
	pos: List[float],
	quat: Optional[List[float]] = None,
	relative: bool = False,
	world_frame: bool = False,
	blocking: bool = True,
	debug: bool = False,
	initial_cfg: np.ndarray = None,
	) -> bool:
	"""Command gripper to pose
	Does not rotate base.
	Cannot be used in navigation mode.
	Args:
	pos: Desired position
	quat: Desired orientation in quaternion (xyzw)
	relative: Whether specified pose is relative to current pose
	world_frame: Infer poses in world frame instead of base frame
	blocking: Whether command blocks until completion
	initial_cfg: Preferred (initial) joint state configuration
	"""
	full_body_cfg = self.solve_ik(pos, quat, relative, world_frame, initial_cfg, debug)
	if full_body_cfg is None:
	print("Warning: Cannot find an IK solution for desired EE pose!")
	return False
	joint_pos = self._extract_joint_pos(full_body_cfg)
	self.goto_joint_positions(joint_pos, blocking=blocking, debug=debug)
	# Debug print
	if debug and blocking:
	pos, quat = self.get_ee_pose()
	print(f"Achieved EE pose: pos={pos}; quat={quat}")
	print("=======================")
	return True

In this way, we can get rid of all the FK/ IK computation on the server side and all the thresholding of motion errors, and leave them all to be taken care of at ROS/ robot side

Look forward to your feedback!

Best,
Anran

[hello-robot-shehab]

Hi @Dipan-Zhang,

Thank you for opening this issue!

What you are proposing makes sense.

I am not sure about the initial choice to use the uncalibrated URDF. We will try to look into it based on your feedback, but it will take us some time.

If you have already made the changes on your end, please feel free to submit a PR!

Best,
Shehab

[Dipan-Zhang]

Hi @hello-robot-shehab

Thanks for replying!

This PR would actually involve significant changes, as all apps with Cartesian-based control currently rely on this functionality. I am keen to contribute to the solution, but to be honest, it also depends on the Hello Robot team's stance on this.

Looking forward to hearing the results from the internal discussion!

Cheers,
Anran

[hello-robot-shehab]

Hi @Dipan-Zhang,

I understand, I did not mean to burden you with a PR! Thank you for bringing it to our attention. We will take a look soon.

For my curiosity, could I ask you to post the commands and steps to reproduce the 15mm deviation? This will help me test the fix in a setup similar to yours. Thank you very much!

Side note: I believe the calibrated URDF calibrates up to the wrist joint. The gripper and fingers might need separate closed loop control using the ArUco tags mounted on them, so if you are seeing the 15mm deviation at the end effector, that might be the reason. If you could clarify where you are seeing the error, that would be great!

Thank you.

Best,
Shehab