Initial Throw Position

The vision system returns the cup angle and depth. We can simply turn the torso (j0) using the cup angle so the throwing arm is in line with the cup, so the problem of determining the initial throw position boils down to picking a release point such that the ball travels the correct distance, specifically the release angles for j1, j3 and j5. To do this, we solve an optimization problem to minimize the distance between the depth and height the ball has reached at time t and the depth and height of the cup. Note that we set both joint velocities and time before throwing to constant values.

The problem is formulated with the following variables -

First, the end-effector positions and end-effector velocities (both for depth and height) are modeled in terms of joint angles and joint velocities, based on the diagram below-

This can be written as 4 constraints, 2 for position and 2 for velocity -

The nonlinear optimizer minimizes the distance between the final ball position (given by projectile motion) and the given cup position. The objective function to do so is shown below -

This lets us solve for the release angles θ, and from there we we can find the ‘loaded’ pose’s joint angles Φ given the equation below -

We use the ipopt nonlinear solver from Python's Pyomo library. The code is as follows -

# robot dimensions and max joint velocities (from https://sceweb.sce.uhcl.edu/harman/A_CRS_ROS_SeminarDay3/UNIT3_3_SAWYER/3_3_1_160219_Sawyer_Advanced_Specs_Non_Confidential.pdf)

base_height = 1.237

hand_length = 0.3

wrist_speed = 3.485

forearm_length = 0.4

elbow_speed = 1.957

arm_length = 0.4

shoulder_speed = 1.328

# constants for solver

height = TABLE_HEIGHT + CUP_HEIGHT/393.7

g = 9.81

# using pyo nonlinear optimizer with robot constraints

model = pyo.ConcreteModel()

# joint angles

model.theta1 = pyo.Var()

model.theta3 = pyo.Var()

model.theta5 = pyo.Var()

model.dr = pyo.Var() # end effector depth

model.hr = pyo.Var() # end effector height

model.vd = pyo.Var() # end effector velocity in the x (depth) direction

model.vh = pyo.Var() # end effector velocity in the z (height) direction

model.t = pyo.Var() # time

# encode robot position and velocity dynamics as constraints

model.Constraint1 = pyo.Constraint(

expr = model.vd == (arm_length * pyo.cos(math.pi/2 + model.theta1) * shoulder_speed) +

(forearm_length * pyo.cos(math.pi/2 + model.theta1 + model.theta3) * (shoulder_speed + elbow_speed)) +

(hand_length * pyo.cos(math.pi/2 + model.theta1 + model.theta3 + model.theta5) * (shoulder_speed + elbow_speed + wrist_speed))

) # set vd as a function of sawyer dimensions, sawyer speeds, joint angles

model.Constraint2 = pyo.Constraint(

expr = model.dr == (arm_length * pyo.sin(math.pi/2 + model.theta1)) +

(forearm_length * pyo.sin(math.pi/2 + model.theta1 + model.theta3)) +

(hand_length * pyo.sin(math.pi/2 + model.theta1 + model.theta3 + model.theta5))

) # set dr as a function of sawyer dimensions, joint angles

model.Constraint3 = pyo.Constraint(

expr = model.hr == (arm_length * pyo.cos(math.pi/2 + model.theta1)) +

(forearm_length * pyo.cos(math.pi/2 + model.theta1 + model.theta3)) +

(hand_length * pyo.cos(math.pi/2 + model.theta1 + model.theta3 + model.theta5))

) # set hr as a function of sawyer dimensions, joint angles

model.Constraint4 = pyo.Constraint(

expr = model.vh == -((arm_length * pyo.sin(math.pi/2 + model.theta1) * shoulder_speed) +

(forearm_length * pyo.sin(math.pi/2 + model.theta1 + model.theta3) * (shoulder_speed + elbow_speed)) +

(hand_length * pyo.sin(math.pi/2 + model.theta1 + model.theta3 + model.theta5) * (shoulder_speed + elbow_speed + wrist_speed))

)) # set vh as a function of sawyer dimensions, sawyer speeds, joint angles

# minimize distance between target coordinates and the coordinates of the ball at some point along its trajectory

model.Objective = pyo.Objective(expr = (model.dr + model.t*model.vd - cup_depth)**2 + (base_height + model.hr + model.t*model.vh - g*(model.t**2)/2 - height)**2)

solver = pyo.SolverFactory('ipopt')

results = solver.solve(model)

theta1 = pyo.value(model.theta1)

theta3 = pyo.value(model.theta3)

theta5 = pyo.value(model.theta5)

dr = pyo.value(model.dr)

hr = pyo.value(model.hr)

vd = pyo.value(model.vd)

# calculate 'loaded' pose joint angles based on release pose and throw time

theta1_0 = theta1 - RELEASE_TIME * shoulder_speed

theta3_0 = theta3 - RELEASE_TIME * elbow_speed

theta5_0 = theta5 - RELEASE_TIME * wrist_speed

With this, we end up with the initial pose of the throwing motion!