I’ve been scouring for existing code to help with developing the gripper in simulation. I was looking for a way to implement ‘eye-in-hand’ visual servoing, and came across a good resource, created for a masters thesis, which shows a ‘robot vision’ window, and he compares depth sensing algorithms. My approach was going to be, essentially, segmentation, in order to detect and localise chickens and eggs, in the field of vision, and then just try get their shape into an X-Y coordinate position, and over a certain size, to initiate interaction.
This one uses an SDF model of a KUKA industrial 6 DOF robot with a two finger gripper, but that has specific rotational movement, that seems maybe different from a simpler robot arm. So it’s maybe a bit overkill, and I might just want to see his camera code.
Miranda’s gripper prototype isn’t a $50k KUKA industrial robot arm. It’s just v.0.1 and got an 11kg/cm MG945, some 5kg/cm MG5010s, and an 1.3kg/cm SG90, and a sucker contraption I found on DFRobot, that can suck eggs.
So, regarding the simulation,this will be on top of the robot, as its head.
So we need an URDF file. Or an SDF file. There’s a couple ways to go with this.
The other resource I’ve found that looks like just what I need, is ur5pybullet
Regarding the ‘visual servoing’, the state of the art appears to be QT-Opt, perhaps. Or maybe RCAN, built on top of it. But we’re not there just yet. Another project specifically uses pybullet. Some extra notes here, from Sergey Levine, and co., associated with most of these projects.
Another good one is Retina-GAN, where they convert both simulation and reality into a canonical format. I’ve also come across Dex-Net before, from UCB.
Everything is very complicated though.
I’ve managed to make an URDF that looks good enough to start with, though. I’ll put everything in a github. We want to put two servos on the ‘head’ for animatronic emotional aesthetics, but there’s a sucker contraption there for the egg, so I think this is good enough for simulation, for now, anyway. I just need to put a camera on its head, put some eggs in the scene, and maybe reward stable contact with the tip. Of course it’s going to be a lot of work.
We also want to add extra leg parts, but I don’t want to use 4 more motors on it.
So I’m playing around with some aluminium and timing belts and pulleys to get 8 leg parts on 4 motors. Something like this, with springs if we can find some.
So, simulator camera vision. I can enable the GUI. Turns out I just need to press ‘g’ to toggle.
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_RENDERING, 1)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_GUI, 1)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 1)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 1)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_RGB_BUFFER_PREVIEW, 1)I’ve added the gripper, and now I’m printing out the _control_observation, because I need to work out what is in an observation.
self.GetTrueMotorAngles() 0.18136442583543283, 0.4339093246887722, -0.25269494256467184, 0.32002873424829736, -0.6635045784503064, 1.5700002984158676, -1.5700000606174402, -0.2723645141027962, self.GetTrueMotorVelocities() 0.451696256678765, 0.48232988947216504, -4.0981980703534395, 0.4652986924553241, 0.3592921211587608, -6.978131098967118e-06, 1.5237597481713495e-06, -10.810712328063294, self.GetTrueMotorTorques() -3.5000000000000004, -3.5000000000000004, 3.5000000000000004, -3.5000000000000004, 3.5000000000000004, -3.5000000000000004, 3.5000000000000004, 3.5000000000000004, self.GetTrueBaseOrientation() -0.008942336195953221, -0.015395612988274186, 0.00639837318132646, 0.9998210192552996, self.GetTrueBaseRollPitchYawRate() -0.01937158793669886, -0.05133982438770338, 0.001050170752804882]
Ok so I need the link state of the end effector (8th link), to get its position and orientation.
state = self._pybullet_client.getLinkState(self.quadruped, self._end_effector_index)
pos = state[0]
orn = state[1]
print(pos)
print(orn)
(0.8863188372297804, -0.4008813832608453, 3.1189486984341848)
(0.9217446940545668, 0.3504950513334899, -0.059006227834041206, -0.1551070696318658)
Since the orientation is 4 dimensions, it’s a quaternion,
def gripper_camera(self, state):
pos = state[0]
ori = state[1]
rot_matrix = self._pybullet_client.getMatrixFromQuaternion(ori)
rot_matrix = np.array(rot_matrix).reshape(3, 3)
# Initial vectors
init_camera_vector = (1, 0, 0) # z-axis
init_up_vector = (0, 1, 0) # y-axis
# Rotated vectors
camera_vector = rot_matrix.dot(init_camera_vector)
up_vector = rot_matrix.dot(init_up_vector)
self.view_matrix_gripper = self._pybullet_client.computeViewMatrix(pos, pos + 0.1 * camera_vector, up_vector)
img = self._pybullet_client.getCameraImage(256, 256, self.view_matrix_gripper, self.projectionMatrix, shadow=0, flags = self._pybullet_client.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL)
Ok I’ve got the visuals now, but I shouldn’t be seeing that shadow
The camera is like 90 degrees off maybe. Could be an issue with the camera setup, or maybe the URDF setup? Ok…
Changing the initial camera vector fixed the view somewhat:
init_camera_vector = (0, 0, 1) # x-axis
Except that we’re looking backwards now.
init_camera_vector = (0, 0, -1) # x-axis
Ok well it’s correct now, but heh, hmm. Might need to translate the camera just a bit higher.
I found a cool free chicken obj file with Creative commons usage. And an egg.
Heh need to resize obj files. Collision physics is fun.
Ok I worked out how to move the camera a bit higher.
pos = list(pos)
pos[2] += 0.3
pos = tuple(pos)
Alright! Getting somewhere.
So, next, I add resized eggs and some chickens for good measure, to the scene.
Then we need to train it to stick its shnoz on the eggs.
Ok… gonna have to train this sucker now.
First, the table is falling from the sky, so I might need to stabilize it first. I also need to randomize the egg location a bit.
And I want to minimize the distance between the gripper attachment and the egg.
The smart way is probably to have it walk until some condition and then grasp, but in the spirit of letting the robot learn things by itself, I will probably ignore heuristics. If I do decide to use heuristics, it will probably be a finite state machine with ‘walking’ mode and ‘gripping’ mode. But we’ll come back to this when it’s necessary. Most of the time there won’t be any eggs in sight. So it will just need to walk around until it is sure there is an egg somewhere in sight.
Ok I’ve added a random egg to the scene
self.rng = default_rng()
egg_position = np.r_[self.rng.uniform(-20, 20, 2), 0.1]
egg_orientation = transformations.random_quaternion(self.rng.random(3))
self._egg_mesh = self._pybullet_client.loadURDF("%s/egg.urdf" % self._urdf_root, egg_position, egg_orientation, globalScaling=0.1)
And the end effector’s position should be something like the original camera position before we moved it up a bit, plus length of the end effector in the URDF (0.618). I ended up doing this:
pos = [pos[0] + 0.5*camera_vector[0],
pos[1] + 0.5*camera_vector[1],
pos[2] + 0.5*camera_vector[2]]
pos = tuple(pos)
And it’s closer to the tip now. But yeah. I will start a new post, Simulation Vision.