Various fixes

[aPonza]

Missing:

• configuration of grip_ready and drop_box from a yaml file;
• fallback on panda_arm if panda_arm_hand move group doesn't have an end effector.

I'm still pushing this hoping you can test if it works for you as well (as it should).

Closes #8

[aPonza]

BTW if you check b2c4470 my intention was to cherry-pick your own changes to the base launch files so that you don't have duplicated code. I might have missed something, let me know if that's the case.

[seivazi]

@aPonza Thank you for sharing these changes. After grasping finish I get following error:

panda_base_grasping_controller.py", line 376, in go
grasp_ret, run = self.execute_best_grasp(run)
TypeError: 'NoneType' object is not iterable

Any suggestion?

[aPonza]

If you retry now that should work. It was a pass-by-object-value issue.

[aPonza]

I'm having trouble understanding the function __execute_best_grasp, would you (@dougsm) please help me understand the code? Starting from the beginning:

mvp_grasp/mvp_grasping/src/mvp_grasping/panda_base_grasping_controller.py

Lines 254 to 260 in c301ba2

	# Offset for initial pose.
	initial_offset = 0.05
	LINK_EE_OFFSET = 0.138
	# Add some limits, plus a starting offset.
	self.best_grasp.pose.position.z = max(self.best_grasp.pose.position.z - 0.01, 0.026) # 0.021 = collision with ground
	self.best_grasp.pose.position.z += initial_offset + LINK_EE_OFFSET # Offset from end efector position to

What do the offsets represent and where do the numbers come from? Why is the z-coordinate manipulated instead of being kept to the calculated one?

Every time I get to the last bit of movement to go and actually grasp the object I obtain an ABORTED: No motion plan found. No execution attempted. MoveIt message, so I grasp thin air. Could it be because my ground level is different than yours?

Where is the camera mount transform defined and how is it related to these offsets? Could it be that LINK_EE_OFFSET = gripper_offset + 0.10339999943971634 from

mvp_grasp/franka_control_wrappers/ros_nodes/set_panda_defaults.py

Lines 28 to 30 in c301ba2

	# This is the default settings + 35mm down for the gripper
	gripper_offset = 0.035
	eef_msg.F_T_EE = [0.707099974155426, -0.707099974155426, 0.0, 0.0, 0.707099974155426, 0.707099974155426, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, gripper_offset + 0.10339999943971634, 1.0]

or is this something else entirely? Does this then mean that 0.035mm is the height of your camera mount?

Afterwards:

mvp_grasp/mvp_grasping/src/mvp_grasping/panda_base_grasping_controller.py

Lines 269 to 278 in c301ba2

	self.cs.switch_controller('velocity')
	v = Twist()
	v.linear.z = -0.05
	# Monitor robot state and descend
	while self.robot_state.O_T_EE[-2] > self.best_grasp.pose.position.z and not any(self.robot_state.cartesian_contact) and not self.ROBOT_ERROR_DETECTED:
	self.curr_velo_pub.publish(v)
	rospy.sleep(0.01)
	v.linear.z = 0
	self.curr_velo_pub.publish(v)

What is the point of moving vertically downwards with a different controller instead of constraining the MoveIt controller to find a solution in the z axis? Is it just a different approach or does it have benefits I didn't consider?

[aPonza]

Be mindful that loading the remembered poses from here means restarting robot_bringup.launch for reloading the pose. This is in my opinion ideal as one would bring up their own robot anyways with their own launch file if not using the complete mvp_grasp workspace.

EDIT: force pushed due to a line being in the wrong commit

EDIT2: forgot to mention I used the same pose that was previously used aspregrasp_pose as the committed grip_ready pose, and something that worked in my case for drop_box, which I can of course change to whatever you want in the final draft.

[aPonza]

Set the scale to 1.0 to remove the RuntimeWarning: divide by zero encountered in true_divide as per #8 (comment)

[jethrokuan]

Hi, I've been trying to replicate this setup on our Franka, with the Franka gripper. Almost everything works as advertised, except for one catch: the best grasp output by the GGCNN service is always rotated 45 degrees on the z-axis

One thing I noticed is that the end-effector link panda_link8 is rotated 45 degrees in the z-axis as well. If I send an orientation of (x,y,z,w) = (1,0,0,0) as a move command to the panda_arm group, the joint state for the end-effector is now at -45 degrees. This also correctly corresponds to the arrow indication on the Franka arm.

Was there any rotational correction required wrt to the gripper orientation from the output of the GGCNN service?

[aPonza]

iirc that's a problem I was having as well, but I've been away from the lab for a long time now. There was something about panda_K that fixed it, it should be the center of the closed fingers, with the correct rotation. Try to check that out.

WRT this PR, I still haven't received answers from the repo owner, so that's why it's still a draft. Also it's afaik untested by him as well, so it might be my setup doesn't work for everyone.