ohmm
Class Grasp

java.lang.Object
  ohmm.Grasp

public class Grasp
extends java.lang.Object

Arm IK and grasping demo (CS4610 LAB3 and 4 solution).

ikDemo() is the top-level driver for interactive arm IK (LAB3 part one).

graspDemo(float, float) is the top-level driver for blind world-fram grasping (LAB3 part two).

vsDemo(int, int, int, int, boolean, boolean) is the top-level driver for visual-servo based grasping (LAB4) (uses CvVisualServo).

See the LAB3 and LAB4 procedures for details.

Many of the component functions are also available for use independently. The companion class CvVisualServo may be used directly for different variations of visual-servo based grasping.

Author:

Marsette Vona

Field Summary

static float	ARM_OFFSET Radial offset in mm from robot frame origin to grip point.
float	d Increment amount in mm for ikDemo().
static float	D_INCR Default increment amount in mm for ikDemo().
static float	D_MAX Increment limits in mm for ikDemo().
static float	D_MIN Increment limits in mm for ikDemo().
static float	EE_TOL EE position tolerance in mm.
float	gx_r Grip point location in mm in robot frame.
float	gx_w Target location in world mm.
float	gy_w Target location in world mm.
float	gz_r Grip point location in mm in robot frame.
float	gz_w Target location in world mm.
static float	HOME_TO_GRIP Extension distance in mm from home to grip.
static float	HOME_TO_GROUND Extension distance in mm from home to ground.
static float	INTERP_SPEED EE interpolation speed in mm/sec for interpolateArm(float, float).
static float	L0 Arm constants in mm.
static float	L1 Arm constants in mm.
OHMM	ohmm The OHMM object.
static float	PI Float cover of Math constant.
static java.lang.String	RXTX_PORT RXTX port filename.
private static java.lang.String	svnid
static float	SX Arm constants in mm.
static float	SZ Arm constants in mm.
static float	T0_HOME Home pose joint angles.
static float	T0_MAX Joint angle limits in rad.
static float	T0_MIN Joint angle limits in rad.
static float	T1_HOME Home pose joint angles.
static float	T1_MAX Joint angle limits in rad.
static float	T1_MIN Joint angle limits in rad.
static float	T2_HOME Home pose joint angles.
static float	T2_MAX Joint angle limits in rad.
static float	T2_MIN Joint angle limits in rad.
float[]	theta Arm joint angles in rad.
static float	THETA_TOL Joint angle tolerance in rad.
float	thetaR Robot orientation in rad.
static java.lang.String	USAGE Usage message.
static float	WX Arm constants in mm.
static float	WZ Arm constants in mm.
float[]	xyt Robot pose in world frame.

Constructor Summary

Grasp()
Inits ohmm on RXTX_PORT.

Grasp(OHMM ohmm)
Inits ohmm.

Method Summary

static float	abs(float t) float cover of eponymous Math API
void	armFK() Update gx_r, gz_r from theta.
boolean	armIK(boolean local) Update theta and thetaR from either gx_w, gy_w, gz_w (global) or gx_r, gz_r (local).
void	armWait() Block until arm motion completes.
static float	atan2(float y, float x) float cover of eponymous Math API
void	cal(boolean includeYaw) Similar to home(boolean) but goes to calibration pose.
static float	cos(float t) float cover of eponymous Math API
static int	div(int n, int d) float division with rounding
void	driveWait() Block until drive motion completes.
void	dumpArm() Dump current theta and gx_r, gz_r.
static java.lang.String	fmt(float f) format a float
static java.lang.String	fmt(float[] f) covers fmt(float, float, float)
static java.lang.String	fmt(float x, float y, float z) format three float
void	graspDemo(float x, float y) Runs the grasping demo.
void	home(boolean includeYaw) Home the arm.
void	ikDemo() Runs the interactive ik demo.
void	ikHelp() Show interactive help for interactive IK.
void	init() Reset drive pose, disable drive orientation servo, then home(boolean).
void	interpolateArm(float dx, float dz) Interpolate vertical plane arm motion along a line in task space.
static void	main(java.lang.String[] argv) Demo driver, see USAGE.
static float	max(float y, float x) float cover of eponymous Math API
static float	min(float y, float x) float cover of eponymous Math API
void	orientAndWait(float toTheta) Yaw robot and wait for motion to finish.
static int	parseInt(java.lang.String s) convienence to parse an int
static void	print(java.lang.String s) convenience to print to System.out
static void	println(java.lang.String s) convenience to print to System.out
static float	sign(float t) get float sign
static float	sin(float t) float cover of eponymous Math API
static float	sqrt(float f) float cover of eponymous Math API
static float	toDeg(float t) float cover of eponymous Math API
static float	toRad(float t) float cover of eponymous Math API
void	updateArm(boolean local, boolean spew) armIK(boolean) and then set the arm to the new theta.
void	vsDemo(int camIndex, int calH, int calX, int calY, boolean localDBG, boolean noServer) Runs the visual servoing demo.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

svnid

private static final java.lang.String svnid

See Also:

Constant Field Values

USAGE

public static final java.lang.String USAGE

Usage message.

See Also:

Constant Field Values

PI

public static final float PI

Float cover of Math constant.

See Also:

Constant Field Values

RXTX_PORT

public static final java.lang.String RXTX_PORT

RXTX port filename.

See Also:

Constant Field Values

THETA_TOL

public static final float THETA_TOL

Joint angle tolerance in rad.

EE_TOL

public static final float EE_TOL

EE position tolerance in mm.

See Also:

Constant Field Values

INTERP_SPEED

public static final float INTERP_SPEED

EE interpolation speed in mm/sec for interpolateArm(float, float).

See Also:

Constant Field Values

T0_MIN

public static final float T0_MIN

Joint angle limits in rad.

T0_MAX

public static final float T0_MAX

Joint angle limits in rad.

T1_MIN

public static final float T1_MIN

Joint angle limits in rad.

T1_MAX

public static final float T1_MAX

Joint angle limits in rad.

T2_MIN

public static final float T2_MIN

Joint angle limits in rad.

T2_MAX

public static final float T2_MAX

Joint angle limits in rad.

T0_HOME

public static final float T0_HOME

Home pose joint angles.

T1_HOME

public static final float T1_HOME

Home pose joint angles.

T2_HOME

public static final float T2_HOME

Home pose joint angles.

SX

public static final float SX

Arm constants in mm.

See Also:

Constant Field Values

SZ

public static final float SZ

Arm constants in mm.

See Also:

Constant Field Values

L0

public static final float L0

Arm constants in mm.

See Also:

Constant Field Values

L1

public static final float L1

Arm constants in mm.

See Also:

Constant Field Values

WX

public static final float WX

Arm constants in mm.

See Also:

Constant Field Values

WZ

public static final float WZ

Arm constants in mm.

See Also:

Constant Field Values

D_INCR

public static final float D_INCR

Default increment amount in mm for ikDemo().

See Also:

Constant Field Values

D_MAX

public static final float D_MAX

Increment limits in mm for ikDemo().

See Also:

Constant Field Values

D_MIN

public static final float D_MIN

Increment limits in mm for ikDemo().

See Also:

Constant Field Values

HOME_TO_GROUND

public static final float HOME_TO_GROUND

Extension distance in mm from home to ground.

See Also:

Constant Field Values

HOME_TO_GRIP

public static final float HOME_TO_GRIP

Extension distance in mm from home to grip.

See Also:

Constant Field Values

ARM_OFFSET

public static final float ARM_OFFSET

Radial offset in mm from robot frame origin to grip point.

See Also:

Constant Field Values

ohmm

public OHMM ohmm

The OHMM object.

d

public float d

Increment amount in mm for ikDemo().

thetaR

public float thetaR

Robot orientation in rad.

theta

public float[] theta

Arm joint angles in rad.

gx_r

public float gx_r

Grip point location in mm in robot frame.

gz_r

public float gz_r

Grip point location in mm in robot frame.

gx_w

public float gx_w

Target location in world mm.

gy_w

public float gy_w

Target location in world mm.

gz_w

public float gz_w

Target location in world mm.

xyt

public float[] xyt

Robot pose in world frame.

Constructor Detail

Grasp

public Grasp()
      throws java.io.IOException

Inits ohmm on RXTX_PORT.

Throws:

java.io.IOException

Grasp

public Grasp(OHMM ohmm)
      throws java.io.IOException

Inits ohmm.

Parameters:

ohmm - an existing OHMM

Throws:

java.io.IOException

Method Detail

main

public static void main(java.lang.String[] argv)
                 throws java.io.IOException

Demo driver, see USAGE.

Throws:

java.io.IOException

ikDemo

public void ikDemo()

Runs the interactive ik demo.

vsDemo

public void vsDemo(int camIndex,
                   int calH,
                   int calX,
                   int calY,
                   boolean localDBG,
                   boolean noServer)

Runs the visual servoing demo.

Parameters:

camIndex - the opencv camera index

calH - the calibration hue or negative to run interactive

calX - the calibration X or negative to run interactive

calY - the calibration Y or negative to run interactive

localDBG - whether to open a local debug window by setting CvBase.useCanvasFrame = true

noServer - whether to inhibit the HTTP image server by setting CvBase.useServer = false

Constructs a CvVisualServo and calls CvBase.mainLoop().

graspDemo

public void graspDemo(float x,
                      float y)

Runs the grasping demo.

Parameters:

x - target x location in world frame mm

y - target y location in world frame mm

ikHelp

public void ikHelp()

Show interactive help for interactive IK.

dumpArm

public void dumpArm()

Dump current theta and gx_r, gz_r.

orientAndWait

public void orientAndWait(float toTheta)

Yaw robot and wait for motion to finish.

Parameters:

toTheta - target orientation in world frame radians

Updates thetaR.

interpolateArm

public void interpolateArm(float dx,
                           float dz)

Interpolate vertical plane arm motion along a line in task space.

Parameters:

dx - change in EE x position in robot frame mm

dz - change in EE z position in robot frame mm

The interpolation starts at the current EE location (gx_r, gz_r) in robot frame and proceeds at INTERP_SPEED mm/sec.

armWait

public void armWait()

Block until arm motion completes.

driveWait

public void driveWait()

Block until drive motion completes.

home

public void home(boolean includeYaw)

Home the arm.

Inits theta by readback from arm after homing, thetaR = 0, and (gx_w = gx_r, gy_w = 0, gz_w = gz_r) in robot frame mm by armFK().

Parameters:

includeYaw - whether to including yawing to orientation 0

cal

public void cal(boolean includeYaw)

Similar to home(boolean) but goes to calibration pose.

init

public void init()

Reset drive pose, disable drive orientation servo, then home(boolean).

updateArm

public void updateArm(boolean local,
                      boolean spew)

armIK(boolean) and then set the arm to the new theta.

armFK

public void armFK()

Update gx_r, gz_r from theta.

armIK

public boolean armIK(boolean local)

Update theta and thetaR from either gx_w, gy_w, gz_w (global) or gx_r, gz_r (local).

Parameters:

local - whether to do local update

Returns:

true unless the target was unreachable

Updates gx_r, gz_r from gx_w, gy_w, gz_w and current robot location when not local.

toDeg

public static float toDeg(float t)

float cover of eponymous Math API

toRad

public static float toRad(float t)

float cover of eponymous Math API

sqrt

public static float sqrt(float f)

float cover of eponymous Math API

cos

public static float cos(float t)

float cover of eponymous Math API

sin

public static float sin(float t)

float cover of eponymous Math API

abs

public static float abs(float t)

float cover of eponymous Math API

sign

public static float sign(float t)

get float sign

div

public static int div(int n,
                      int d)

float division with rounding

atan2

public static float atan2(float y,
                          float x)

float cover of eponymous Math API

max

public static float max(float y,
                        float x)

float cover of eponymous Math API

min

public static float min(float y,
                        float x)

float cover of eponymous Math API

fmt

public static java.lang.String fmt(float f)

format a float

fmt

public static java.lang.String fmt(float x,
                                   float y,
                                   float z)

format three float

fmt

public static java.lang.String fmt(float[] f)

covers fmt(float, float, float)

println

public static void println(java.lang.String s)

convenience to print to System.out

print

public static void print(java.lang.String s)

convenience to print to System.out

parseInt

public static int parseInt(java.lang.String s)
                    throws java.lang.NumberFormatException

convienence to parse an int

Throws:

java.lang.NumberFormatException