Using Theseus to generate a full 3D trajectory of a 7DOF robot arm via motion planning

[DanielTakeshi]

After going through some of the Theseus tutorials, here's what I can piece together:

• From the tutorials 04 and 05, those use motion planning in a 2D environment to avoid collisions. They produce a trajectory which specifies 2D positions and 2D velocities of the "robot" (which is really a point mass) at each time step. Thus, if a trajectory lasts 100 time steps, Theseus needs to find values for optimization variables of positions and velocities for each of the 100 time steps. This helps it avoid collisions, etc.
• There is an inverse kinematics example: https://github.com/facebookresearch/theseus/blob/main/examples/inverse_kinematics.py Given a target pose of the robot EE (position and quaternion) we want to find the joint angles that will get the EE to that pose. This produces one set of joint angles for each target, so it's not specifying a full trajectory, it's just producing the IK solution.

Now consider the mix of the two. Instead of a 2D map and a "point mass" robot as in tutorials 04, and 05, what if we have a full robot arm like the Franka, as in the inverse kinematics examples. However, instead of finding one joint angle, we want to find joint angles for all time steps in a trajectory, such that the full robot arm can avoid any collisions we insert in the environment. Here the environment would be 3D with obstacles, so the point of motion planning is to find a full 3D collision-free trajectory.

Questions:

• Would Theseus be a good use case for this idea? I see that GPMP (tutorial 04) tested on a 7DOF arm planning problem https://homes.cs.washington.edu/~bboots/files/GPMP.pdf but this was before Theseus was developed.
• I am assuming there is not a quick example script ready to show for this. Would it make sense to try and build upon the previously mentioned points (the tutorials and the IK example) to implement this idea?

Thanks again for the great library!

[mhmukadam]

Hi @DanielTakeshi, that's a great insight, what you suggest is indeed possible.

Q1: Yes, Theseus is a good use case for this, especially if you want a differentiable/batchable planner. The closest prior work you can reference is GPMP2 (follow up of the GPMP work you mentioned) with its associated cpp code (matlab tutorial and python example).

Q2: This has been something on our stack of things but we haven't been able to prioritize. If you are interested in giving it a try we can discuss some of the details. You can already easily put together a free-space arm planner using the GPMotionModel cost if the goal is in joint space or use FK if the goal is in end effector space. For 3D collision avoidance to build the full arm planner, two key things need to be added: (i) FK with body points / spheres and (ii) extending the 2D collision cost and SDF support to 3D.

[fantaosha]

If I understand correctly, you would like a custom root pose other than identity. Is this correct? Thanks!

[DanielTakeshi]

Thanks @mhmukadam , there is one thing I probably want to do before I submit a PR: figure out a good "initial" trajectory. I think in my code the joint angles start at all 0 (though I have to double check) and it might be helpful to try a better initialization scheme.

@fantaosha Basically what I'd like to do is something like I show in this video https://www.youtube.com/watch?v=b-fArLteqZQ but where the robot base (or root) starts at a different pose. In the below code from IK, we are loading robot from the URDF and then defining a forward kinematics function from that.

theseus/examples/inverse_kinematics.py

Lines 28 to 43 in dd4356b

	URDF_REL_PATH = "../tests/theseus_tests/embodied/kinematics/data/panda_no_gripper.urdf"
	urdf_path = os.path.join(os.path.dirname(__file__), URDF_REL_PATH)
	robot = Robot.from_urdf_file(urdf_path, dtype)
	link_names = ["panda_virtual_ee_link"]
	# We can get differentiable forward kinematics functions for specific links
	# by using `get_forward_kinematics_fns`. This function creates three differentiable
	# functions for evaluating forward kinematics, body jacobian and spatial jacobian of
	# the selected links, in that order. The return types of these functions are as
	# follows:
	#
	# - fk: returns a tuple of link poses in the order of link names
	# - jfk_b: returns a tuple where the first is a list of link body jacobians, and
	# the second is a tuple of link poses---both are in the order of link names
	# - jfk_s: same as jfk_b except returning the spatial jacobians
	fk, jfk_b, jfk_s = get_forward_kinematics_fns(robot, link_names)

Is there a way to do something like: robot.adjust_base_pose( translation, orientation ) so that we can adjust the robot (base, or "root") pose before calling get_forward_kinematics_fns, and then have the forward kinematics function accurately reflect that? For example if we adjusted the base pose so that the z coordinate of the robot base starts at 0.1 meters above ground, then all the forward kinematics functions for EE poses should result in z coordinate values of +0.1 compared to the default robot pose.

I'm not sure how easy/hard it would be to integrate in the current code, so just inquiring about this. Thanks!

[mhmukadam]

Sounds good on the PR. For traj initialization there are a few things we have tried in the past (each has it's pros/cons): (1) straight line in joint angle space between start and goal (possible if you have a joint goal), (2) all traj is at the start joint angles, (3) random/zero init.

[DanielTakeshi]

@fantaosha Just revisiting this discussion, do you know if there is function to support "a custom root pose other than identity"?

[fantaosha]

Hi @DanielTakeshi, sorry for the late replay. Unfortunately, there is no such function to set the root pose. But I think it is quite straightforward to write such a function by yourselves using group composition and adjoint transformation. Thanks!