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

Fields
Modifier and Type	Field and Description
`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

Constructors
Constructor and Description

Grasp()
Inits ohmm on RXTX_PORT.

Grasp(OHMMDrive ohmm)
Inits ohmm.

Constructors
Constructor and Description
`Grasp()` Inits `ohmm` on `RXTX_PORT`.
`Grasp(OHMMDrive ohmm)` Inits `ohmm`.

Method Summary

Methods
Modifier and Type	Method and Description
`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

Class Grasp

Field Summary

Constructor Summary

Method Summary

Methods inherited from class java.lang.Object

Field Detail

svnid

USAGE

PI

RXTX_PORT

THETA_TOL

EE_TOL

INTERP_SPEED

T0_MIN

T0_MAX

T1_MIN

T1_MAX

T2_MIN

T2_MAX

HOME

SALUTE

STOW_UP

STOW_DOWN

SX

SZ

L0

L1

WX

WZ

D_INCR

D_MAX

D_MIN

HOME_TO_GROUND

HOME_TO_GRIP

ARM_OFFSET

ohmm

d

thetaYaw

theta

gx_r

gz_r

gx_w

gy_w

gz_w

xyt

grip

armEnabled

stowUp

Constructor Detail

Grasp

Grasp

Method Detail

main

ikDemo

graspDemo

ikHelp

dumpArm

orientAndWait

interpolateArm

armWait

driveWait

home

home

cal

stow

stow

salute

init

updateArm

updateArm

updateArm

armFK

armFK

armIK

armIK

toDeg

toRad

sqrt

cos

sin