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).

Many of the component functions are also available for use independently.

Author:

Marsette Vona

Field Summary

static float	ARM_OFFSET Radial offset in mm from robot frame origin to grip point.
protected boolean	armEnabled Whether arm is enabled.
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	grip Gripper state.
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 Home pose joint angles.
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.
OHMMDrive	ohmm The OHMMDrive object.
static float	PI Float cover of Math constant.
static java.lang.String	RXTX_PORT RXTX port filename.
static float[]	SALUTE Salute pose joint angles.
static float[]	STOW_DOWN Stow joint angles.
static float[]	STOW_UP Stow joint angles.
protected boolean	stowUp Whether next stow should be in up position.
private static java.lang.String	svnid
static float	SX Arm constants in mm.
static float	SZ Arm constants in mm.
static float	T0_MAX Joint angle limits in rad.
static float	T0_MIN Joint angle limits in rad.
static float	T1_MAX Joint angle limits in rad.
static float	T1_MIN Joint angle limits in rad.
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	thetaYaw 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(OHMMDrive ohmm)
Inits ohmm.

Method Summary

static float	abs(float t) float cover of eponymous Math API
void	armFK() Covers armFK(boolean), always local.
void	armFK(boolean local) Update fields from current hardware state.
boolean	armIK() Covers armIK(boolean), always local.
boolean	armIK(boolean local) Update joint angle fields from target pose fields.
void	armWait() Block until arm motion completes.
static float	atan2(float y, float x) float cover of eponymous Math API
void	cal() 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() Covers home(boolean), always local.
void	home(boolean local) Home the arm, then armFK(boolean).
void	ikDemo() Runs the interactive ik demo.
void	ikHelp() Show interactive help for interactive IK.
void	init() Disable drive orientation servo, reset hardware odomery, reinit and enable arm hardware, and home(boolean) (global).
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
void	salute() Salute the arm, then armFK(boolean).
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
void	stow() Covers stow(boolean), down.
void	stow(boolean up) Stow the arm, then armFK(boolean).
static float	toDeg(float t) float cover of eponymous Math API
static float	toRad(float t) float cover of eponymous Math API
void	updateArm() Covers updateArm(boolean, boolean), always local, no spew.
void	updateArm(boolean spew) Covers updateArm(boolean, boolean), always local.
void	updateArm(boolean local, boolean spew) armIK(boolean) then set arm hardware to the new theta.

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.

HOME

public static final float[] HOME

Home pose joint angles.

SALUTE

public static final float[] SALUTE

Salute pose joint angles.

STOW_UP

public static final float[] STOW_UP

Stow joint angles.

STOW_DOWN

public static final float[] STOW_DOWN

Stow 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 OHMMDrive ohmm

The OHMMDrive object.

d

public float d

Increment amount in mm for ikDemo().

thetaYaw

public float thetaYaw

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.

grip

public float grip

Gripper state.

armEnabled

protected boolean armEnabled

Whether arm is enabled.

stowUp

protected boolean stowUp

Whether next stow should be in up position.

Constructor Detail

Grasp

public Grasp()
      throws java.io.IOException

Inits ohmm on RXTX_PORT.

Throws:

java.io.IOException

Grasp

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

Inits ohmm.

Parameters:

ohmm - an existing OHMMDrive

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.

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 xyt and thetaYaw.

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 (updated by a call to armFK(boolean), local FK) 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 local)

Home the arm, then armFK(boolean).

Parameters:

local - whether to home only the arm joints (local) or also to include the robot yaw (global)

home

public void home()

Covers home(boolean), always local.

cal

public void cal()

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

stow

public void stow(boolean up)

Stow the arm, then armFK(boolean).

Parameters:

up - whether to stow in up position

stow

public void stow()

Covers stow(boolean), down.

salute

public void salute()

Salute the arm, then armFK(boolean).

init

public void init()

Disable drive orientation servo, reset hardware odomery, reinit and enable arm hardware, and home(boolean) (global).

updateArm

public void updateArm(boolean local,
                      boolean spew)

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

updateArm

public void updateArm(boolean spew)

Covers updateArm(boolean, boolean), always local.

updateArm

public void updateArm()

Covers updateArm(boolean, boolean), always local, no spew.

armFK

public void armFK(boolean local)

Update fields from current hardware state.

Always updates theta from the hardware, then gx_r, gz_r.

Global FK also updates xyt from hardware odometry, then thetaYaw, gx_w, gy_w, gz_w.

Parameters:

local - whether to do local FK, else global

armFK

public void armFK()

Covers armFK(boolean), always local.

armIK

public boolean armIK(boolean local)

Update joint angle fields from target pose fields.

Updates theta from

• gx_w, gy_w, gz_w for global IK
• gx_r, gz_r for local IK

Global IK also updates xyt from hardware odometry, then gx_r, gz_r and thetaYaw.

Parameters:

local - whether to do local IK, else global

Returns:

true unless the target was unreachable

armIK

public boolean armIK()

Covers armIK(boolean), always 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