David MacNair, Eohan George, Pascal Minnerup, Raghvendra Cowlagi
A set of objects (like blocks) are placed on a table. An overhead camera can detect their positions and orientations. The robot arm's task is to move the object(s) around to (1) identify kinematic relationships between the objects (e.g., if two blocks are connected by a hinge or by a rigid joint) and (2) move the object(s) to some specified goals. In other words, the arm must learn the kinematic constraints that exist, and then produce a plan which obeys these constraints.
- Dov Katz and Oliver Brock , Extracting Planar Kinematic Models Using Interactive Perception . The paper describes about interleaving perception with physical interactions. Actions are generated that are useful for both building kinematic models and target execution. The paper describes an experiment conducted to identify revolute and prismatic joints in day-to-day objects like scissors. Key points on the object are tracked between frames and min-cut algorithm is used to identify “likely” groups. This groups are subsequently strengthened to identify a rigid body.
- K. M. Lynch , The mechanics of fine manipulation by pushing . Publication deals with modelling and planning of the motion of a body in response to pushes at different positions of the body.
- Jonathan Scholz, Learning and Planning for Open-Ended Robot Manipulation. The paper describes a technique to organize the objects on a table top. For example, an optimization criterion can be moving every object to a some specified orientation. For achieving this, the technique uses a general evaluation function of the current state of the involved objects. The presented algorithm uses a black box view on the used objects/boxes. Using RRT it can find paths to move the boxes to an optimal position with respect to the evaluation function. For moving the objects the paper considers exactly six different possible pushes on the object.
- Mathew Mason and Kenneth Salisbury, Mechanics of Pushing. Theory of Pushing
- Planning Challenges
The main planning challenge is that a motion model of the object is not known a priori. For example, the motion of two independent rods will be very different from their motion when they are connected by a revolute joint (to form a scissor-like object). The planner does not know which of the configurations is true; it only sees two bars (via an overhead camera).
- Proposed Solutions
We will create simple motion model of objects which the planner tries to learn in order to plan further. In other words, we will develop an iterative process: the manipulator executes a step in the current plan, observes the effect of this step, updates its motion model of the objects, and then correspondingly updates its plan.
The motion model is basically an equation which gives the incremental change in position and orientation when the object is pushed by a small amount. This equation will have involve parameters; and any particular object will be described in terms of specific values of these parameters. The idea is to try and guess the values of these parameters for the object that the robot is pushing. The way to do this is to make a push, observe the difference between the estimated and actual changes in position and orientation, and then update the estimated values of the parameters in the motion model.
Given the motion model of a single object with no kinematic relationships with other objects, it will be easier to identify such relationships between objects, again based on the the errors between the estimated and observed motions. For planning, we will use a modification of RRT. The modification will essentially account for the fact that the planner is learning and planning at the same time. There will have to be an element of replanning according to the updated motion model.
Overall, we expect the final algorithm to work as follows: the manipulator will start off with small pushes as it tries to learn the nature of the object(s) placed on the table. At first, its plan will be way off what would be required, but as the motion model gets updated, the plan gets better and better, and the manipulator makes larger pushes.
Jon has been working on a similar problem (independent blocks, motion model = tabulated information about a predetermined set of pushes). We aim to build on his work, e.g., we assume that the controller for the robot arm itself is available and can perform whatever pushing action we command; we assume that the overhead camera can identify shapes.
- Implementation Challenges
One of the issues is that the robot arm blocks the overhead camera. This implies that the planner cannot observe the motion while the arm is pushing - the arm has to be moved away from the table to allow the camera to see the object configuration. One of the ways to get around this problem is to mount the camera at an angle (instead of overhead). But this creates an additional issue of obtaining planar information from perspective image.
- Proposed Solutions
SLAP for a single object: Video. In this video:
1. Planner does not know motion model a priori, i.e., it does not know how much the object will translate/rotate for a given push.
2. Planner can push at arbitrary points on the edge of the block, i.e., pushing is not restricted to a fixed number of predetermined points.
3. Planner works for any block with no additional preprocessing.
4. Planner and controller work for multiple boxes and prevents them from colliding. [http://blip.tv/file/2949202]
|1||Create wiki, create presentation, survey literature.||Motion Model Slides||Related Work Slides||Motivation, Problem Statement Slides||Motion Modell Slides|
|2||Work out simple motion models for pushing (perhaps based on Mason's paper/book) . Work out a basic learning/planning algorithm for identifying kinematic relationships between objects (based on Katz paper) while trying to execute a plan/replan. Setup up and dig into Jon's code for RST and arm control|
|3||Test and fine tune motion model through experiments. Implement RST for continuous spaces as opposed to discrete 6 motions|
|4||Get arm to follow a path using discrete steps. No joints friction motion model. No friction joint motion model.|
|5||Get a planner to work that does not incorporate learning. All joint/friction properties are known a priori.|
|6||Finish the a priori planner. Start implementing learning functions on top of the a priori. Start paper outline.|
|7||Finish the learning planner. Finalize code. Flesh out paper.|
|8||Finalize paper, make ppt, make videos, practice presenting.|
Created wiki and project proposal presentation; brief literature review; started to get familiar with Jon's code; created simple motion model and rudimentary simulation.
Worked on RIP project2; Continued researching the motion model
Tried again to compile Jon's code simultanously on two systems. We first resolved easy solvable problems like missing source files. After that Jon helped us to solve problems with typedefinitions. After a couple of hours we were finally able to compile the code. The next step will be resolving the errors which occur when running the executable.
The errors that occured when running the executable were probably due to the fact that I ran it on a virtual machine. Therefore I installed a new operating system and installed Jon's code there as well. Several new errors occured when I installed the code. I documented the necessary steps to compile and run the simulator including the fixes of the error that occured:
1. Install Linux (e.g. Ubuntu) on you computer (Virtual Machine does not work)
2. Install Eclipse
3. Install Eclipse-CDT. Normally this should work by "sudo apt-get install eclipse-cdt". However it did not in my case. I downloaded it from http://download.eclipse.org/tools/cdt/releases/galileo instead.
4. Import Jon's project into the workspace.
5. Install openCV 1.0 (2.0 does not work with Jon's code) You can get OpenCV at: http://sourceforge.net/projects/opencvlibrary/files/opencv-unix/1.0/opencv-1.0.0.tar.gz/download
6. My compiler at Ubuntu ran into an error when compiling it first. The bug is known: http://bugs.debian.org/cgi-bin/bugreport.cgi?bug=504831
To fix the problem, you have to change the "#elif" in opencv-1.0.0/cxcore/include/cxmisc.h line 133 to "#else"
7. Go to the folder NN. Add the make target "linux-g++", if you work on linux. Else open the makefile and find out the right target. Execute this make target.
8. Install glew (sudo apt-get install libglew1.5-dev)
9. Install glut (sudo apt-get install libglut3-dev)
10. Install fann. My packet-manager only offered me version1. For Jon's code you need version 2: http://leenissen.dk/fann/ The provided Deb-installer did not work with my 64 Bit operating system. Hence I used the source code version. As we do not need the functions connected to fann, another alternative is to comment out all links to fann. However this took about as long as installing fann.
10. In the src Folder run the make targets new and deploy
When trying to do that I got the error "error: ‘sprintf’ was not declared in this scope" To fix this I added #include<stdio> in src/srDyn/srObject.cpp as well as in srIRSensor.cpp, srRangeFinder.cpp, srg/srgBitmap.cpp The same error occured in my system in other folders like jvfeatures.cpp, jvtest.cpp, RRT.h
This error seems to depend on the operating system, as it did not occur on my virtual suse linux.
11. Run the make target simSplinter in the folder simSplinter
12. Run ldconfig in your terminal
13. Run the executable
To start the simulation/planner hit the key "P". When doing so in my system, I get the error "Could not open file: No such file or directory". However the planner starts to work and eventually puts out a goal configuration in the terminal. After that the program terminates without visualizing the plan.
- Changed the workspace controller such that it is well usable for testing to move objects around. We can now also use a trajectory controller to move the end effector at a given trajectory.
- Played around with OpenCV. Now able to detect boxes and centre of rectangular box.
- Created simple model of object that is connected by joints. Experimented with pushing around several objects.
- Start to create class to represent general objects in cpp
- Identifying methods for representing pushes to body. Planning to represent generic pushes on objects with
1. Perpendicular distance from centroid
2. Angle wrt object
3. Contact point coordinates
- Numerical Solution linking desired point of rotation to force line of action solved for a single block. Results are in the matlab file findForce.m. This will now be expanded to multiple blocks with a pin joint linking them and will be used to determine the best path forward for the arm to take to reach the goal configuration.
- Notes about findForce.m are as follows:
This is how to apply forces given a desired point of rotation (eg. a pair of linear velocity and angular velocity) for an object on a table experiencing planer columb friction. The solution takes advantage of a Quasi-Static model thus the inertia of the blocks are not apart of the solution. So long as we push the blocks slowly enough, and accelerate them slowly enough, friction forces will dominate and we will not need to worry about this.
Unfortunately, the solution is numerical in nature and cannot easily be backsolved (the double integrals are too hard for Mathmatica or Maple to solve). I have written a Matlab file for you to try out if you would like which is on the SVN. It only solves the problem for a rectangular block of uniform density and with a constant coef. of friction. These constraints can be changed if needed. The file is a function called using:
Which takes in:
vx, and vy = Linear velocity of the center (0,0) in the x and y directions
w = angular velocity in degrees
xlength = width
ylength = height
Fay = Y component of the force required for the desired motion
Fax = X component of the force required for the desired motion
Mom = Moment that force F=[Fax;Fay] has to achieve wrt. the center (0,0) to achieve the motion
The line of action that you must push along can be found using some trig.:
If the Mom==0, Line of action passes through the center (0,0) and is in the direction of unit vector F/||F||
Otherwise Line of action passes through pt. [ cos(phi)*(abs(Mom)/sqrt(Fax^2+Fay^2)) ; sin(phi)*(abs(Mom)/sqrt(Fax^2+Fay^2) ] and is in the direction of unit vector F/||F||
If you want to push perpendicularly to an edge, then you will want the linear velocity to be entirely in the direction you want to push and you will have to figure out what ratio of linear velocity components and angular velocity gives a line of action that is actually on the object. My tool plots the results to show the line of action you can push along. As long as we push along the curve with center points matching to the object's point of rotation, we should be able to effectively control the object using only displacement and not needing to know the forces. The displacement will just always be assumed to be along the line of action of the force.
- Implemented code to map from workspace coordinate to image coordinate using a Direct Linear Transformation. The calibration (transformation matrix ) is obtained by moving along 3 corners of the workspace and clicking the corresponding points on the video. We can identify the coordinates from 3 points for an affine transformation.
- Implemented code to move robot arm with varying end effector rotations. This ensures that maximum extension of the end effector is obtained, so as to cover the maximum area on the table.
Tested motion model for single boxes using the robot arm.
Started to implement rrt-Methods
- implemented reverse motion model function for rrt purposes
- switched to second robot arm
Executed rrt planner with one box:
the controller can now handle multiple boxes on the table. It is still only pushing one of them, but it is already keeping track of all boxes and it is not distracted by other boxes on the table. The algorithm to distinguish the boxes tries all permutations of expected boxes to observes boxes and adds up the distance of each pair. It will return the mapping with the least distance. If the average distance is bigger than 5 centimeters, it will ask the user, whether this is correct. If the user rejects it, the vision will be queried again.
The result can be seen here: [http://blip.tv/file/2933037]
The video shows one push against a box. The arm ignored the other black boxes, although both are visible for the vision code.
The controller can now plan with and push multiple boxes on a table. The result can be seen here: [http://blip.tv/file/2943793]
The collision detection is not implemented yet. This is why The endeffecter collides with the second box in the end which unintendetly moves that box my several centimeters. Note, that directly after that push, the controller corrects that error and pushes the box to its final target position in only one push.
Planner and controller can no deal with multiple boxes, which optimal paths would collide. In this video: [http://blip.tv/file/2949202] the planner exchanges the positions of two boxes using rrt and a collision detection.
Test of pushing blocks without motion model. In this video [http://blip.tv/file/2949309] the controller tries to push two joint boxes from a start position in the left part of the table to a goal position on the right part of the table. The goal position for both boxes is the same, hence it can't physically achieve this goal. However, it tries to get as near as possible. The video shows, that the controller is robust enough against disturbance, that it can move both boxes, although they do not follow the expected motion model.