Mistakenly taking velocity profile as orientation target

[zichunxx]

Hi! @studywolf Thanks for your amazing repo!

For force_osc_xyz_linear_path_gaussian_velocity.py, the generated path by

abr_control/examples/Mujoco/force_osc_xyz_linear_path_gaussian_velocity.py

Lines 58 to 60 in 0f6e476

	path_planner = PathPlanner(
	pos_profile=Linear(), vel_profile=Gaussian(dt=dt, acceleration=max_a)
	)

is six dimensions containing the 3-D position target and 3-D velocity profile.

However, the filtered_target is taken as the target for osc controller, and the last 3-d velocity profile is taken as the orientation target.

abr_control/examples/Mujoco/force_osc_xyz_linear_path_gaussian_velocity.py

Lines 91 to 104 in 0f6e476

	filtered_target = path_planner.next()
	interface.set_mocap_xyz("target_orientation", filtered_target[:3])
	feedback = interface.get_feedback()
	hand_xyz = robot_config.Tx("EE", feedback["q"])
	if glfw.window_should_close(interface.viewer.window):
	break
	u = ctrlr.generate(
	q=feedback["q"],
	dq=feedback["dq"],
	target=filtered_target,
	)

Is that right? Thanks for your time!

[p3jawors]

Hi @zichunxx, this is partially correct. You've actually found a small bug, thanks for that. What's happening here is that
interface.set_mocap_xyz("target_orientation", filtered_target[:3])
is setting the orientation of the red box that is used to show the position and orientation of the target the arm is following. This is just for visual purposes and does not affect the control at all. Since the velocities are fairly low the orientation does not change much for the target box.

The position and velocity information get passed to the controller in the ctrlr.generate() line when you pass in the filtered_target. If you would like to control orientation as well you would have to update the ctrlr_dof parameter. The first three entries are for x, y, and z, and the last three are for alpha, beta, and gamma orientations. In this example script it's just controll the position since
ctrlr_dof = [True, True, True, False, False, False].

If you look at the [force_osc_xyzabg_linear_position_gaussian_velocity.py] script you can see an example of controlling position and orientation. However, note that due to the design of the jaco2 wrist it will face singularities that make it difficult for the arm to follow all 6DOF at all times.

[zichunxx]

Hi @p3jawors, thanks for your detailed explanation! I have read the example you mentioned.

This is just for visual purposes and does not affect the control at all. Since the velocities are fairly low the orientation does not change much for the target box.

As you said, this bug is logically incorrect but has no major impact on the controller.

Finally, thanks for your patience.