def test_fn(test, reference_corners, reference_axes):
box_origin, box_orient, box_dim = test['origin'], test['orient'], test[
'dim']
box_H = rt.rpyxyz2H(box_orient, box_origin)
box_corners, box_axes = rt.BlockDesc2Points(box_H, box_dim)
if rt.CheckBoxBoxCollision(box_corners, box_axes, reference_corners,
reference_axes):
return True
return False
def ForwardKin(self,ang): ''' inputs: joint angles outputs: joint transforms for each joint, Jacobian matrix ''' # Put you FK code from Homework 1 here self.q[0:-1]=ang for i in range(len(self.q)): angle = [dir*self.q[i] for dir in self.axis[i]] self.Tjoint[i] = rt.rpyxyz2H(angle, np.zeros(3)) if i==0: self.Tcurr[i] = np.matmul(self.Tlink[i], self.Tjoint[i]) else: prev_eff = np.matmul(self.Tcurr[i-1], self.Tlink[i]) self.Tcurr[i] = np.matmul(prev_eff, self.Tjoint[i]) # TODO: Compute Jacobian matrix # Slide 25 for i in range(len(self.Tcurr) - 1): rotate_x , rotate_y, rotate_z = self.axis[i] p=self.Tcurr[-1][0:3,3]-self.Tcurr[i][0:3,3] if abs(rotate_z): a=self.Tcurr[i][0:3,2] elif abs(rotate_y): a=self.Tcurr[i][0:3,1] else: a=self.Tcurr[i][0:3,0] self.J[0:3,i]=np.cross(a,p) self.J[3:7,i]=a return self.Tcurr, self.J
def IterInvKin(self, ang, TGoal):
# Put you IK code from Homework 1 here
'''
inputs: starting joint angles (ang), target end effector pose (TGoal)
outputs: computed joint angles to achieve desired end effector pose,
Error in your IK solution compared to the desired target
'''
self.ForwardKin(ang)
# tune hyperparameters
loops = 10000
transpose_approach = False
limit_angle = 0.5
# For Jacobian Transpose Approach
alpha = 0.05
Err = [0, 0, 0, 0, 0,
0] # error in position and orientation, initialized to 0
for s in range(loops):
#TODO: Compute rotation error
rErrR = np.matmul(TGoal[0:3, 0:3],
np.transpose(self.Tcurr[-1][0:3, 0:3]))
rErrAxis, rErrAng = rt.R2axisang(rErrR)
# limit angle
if rErrAng > limit_angle:
rErrAng = limit_angle
if rErrAng < -limit_angle:
rErrAng = -limit_angle
rErr = [ErrAxis * rErrAng for ErrAxis in rErrAxis]
#TODO: Compute position error
xErr = TGoal[0:3, 3] - self.Tcurr[-1][0:3, 3]
if np.linalg.norm(xErr) > 0.01:
xErr = xErr * 0.01 / np.linalg.norm(xErr)
#TODO: Update joint angles
Err[0:3] = xErr
Err[3:6] = rErr
if transpose_approach:
# Jacobian Transpose Approach (Slide 33)
self.q[0:-1] = self.q[0:-1] + alpha * np.matmul(
np.transpose(self.J), Err)
else:
# Jacobian Pseudo-Inverse Approach (Slide 45)
self.q[0:-1] = self.q[0:-1] + np.matmul(
np.matmul(
np.transpose(self.J),
np.linalg.pinv(np.matmul(self.J, np.transpose(
self.J)))), Err)
#TODO: Recompute forward kinematics for new angles
self.ForwardKin(self.q[0:-1])
return self.q[0:-1], Err
def DetectCollision(self, ang, pointsObs, axesObs):
# implement collision detection using CompCollisionBlockPoints() and rt.CheckBoxBoxCollision()
self.CompCollisionBlockPoints(ang)
for i in range(len(self.Cpoints)):
for j in range(len(pointsObs)):
if rt.CheckBoxBoxCollision(self.Cpoints[i], self.Caxes[i],
pointsObs[j], axesObs[j]):
return True
return False
def CompCollisionBlockPoints(self,ang): # Use your FK implementation here to compute collision boxes for the robot arm # Use code from Homework 2 # slide 54, lecture 9 self.ForwardKin(ang) # Compute current collision boxes for arm for i in range(len(self.Cdesc)): self.Tcoll[i] = np.matmul(self.Tcurr[self.Cidx[i]], self.Tblock[i]) self.Cpoints[i], self.Caxes[i] = rt.BlockDesc2Points(self.Tcoll[i], self.Cdim[i])
def DetectCollisionEdge(self, ang1, ang2, pointsObs, axesObs): # Detects if an edge is valid or in collision for s in np.linspace(0,1,10): ang= [ang1[k]+s*(ang2[k]-ang1[k]) for k in range(len(ang1))] self.CompCollisionBlockPoints(ang) for i in range(len(self.Cpoints)): for j in range(len(pointsObs)): if rt.CheckBoxBoxCollision(self.Cpoints[i],self.Caxes[i],pointsObs[j], axesObs[j]): return True return False
def __init__(self): #Robot descriptor from URDF file (rpy xyz for each link) self.Rdesc=[ [0, 0, 0, 0.0973, 0, 0.1730625], # From robot base to joint1 [0, 0, 0, 0, 0, 0.04125], [0, 0, 0, 0.05, 0, 0.2], [0, 0, 0, 0.2002, 0, 0], [0, 0, 0, 0.063, 0.0001, 0], [0, 0, 0, 0.106525, 0, 0.0050143] # From joint5 to end-effector center ] #Define the axis of rotation for each joint - use your self.axis from Homework 1 self.axis= [ [0, 0, 1], [0, 1, 0], [0, 1, 0], [0, 1, 0], [-1, 0, 0], [0, 1, 0] ] # joint limits self.qmin=[-1.57, -1.57, -1.57, -1.57, -1.57] self.qmax=[1.57, 1.57, 1.57, 1.57, 1.57] #Robot collision blocks descriptor (base frame, (rpy xyz), length/width/height # NOTE: Cidx and Cdesc are just the robot's link BB's self.Cidx= [1,2,3,4] # which joint frame the BB should be defined in # xyz rpy poses of the robot arm blocks (use to create transforms) self.Cdesc=[[0.,0.,0., 0., 0., 0.09], [0.,0.,0., 0.075, 0.,0.], [0.,0.,0., 0.027, -0.012, 0.], [0.,0.,0., 0.055, 0.0, 0.01], ] # dimensions of robot arm blocks self.Cdim=[[0.05,0.05, 0.25], [0.25,0.05,0.05], [0.07,0.076,0.05], [0.11, 0.11, 0.07], ] #Set base coordinate frame as identity self.Tbase= [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]] #Initialize matrices self.Tlink=[] #Transforms for each link (const) self.Tjoint=[] #Transforms for each joint (init eye) self.Tcurr=[] #Coordinate frame of current (init eye) for i in range(len(self.Rdesc)): self.Tlink.append(rt.rpyxyz2H(self.Rdesc[i][0:3],self.Rdesc[i][3:6])) self.Tcurr.append([[1,0,0,0],[0,1,0,0],[0,0,1,0.],[0,0,0,1]]) self.Tjoint.append([[1,0,0,0],[0,1,0,0],[0,0,1,0.],[0,0,0,1]]) self.Tlink[0]=np.matmul(self.Tbase,self.Tlink[0]) self.J=np.zeros((6,5)) self.q=[0.,0.,0.,0.,0.,0.] self.ForwardKin([0.,0.,0.,0.,0.]) self.Tblock=[] #Transforms for each arm block self.Tcoll=[] #Coordinate frame of current collision block self.Cpoints=[] self.Caxes=[] for i in range(len(self.Cdesc)): self.Tblock.append(rt.rpyxyz2H(self.Cdesc[i][0:3],self.Cdesc[i][3:6])) self.Tcoll.append([[1,0,0,0],[0,1,0,0],[0,0,1,0.],[0,0,0,1]]) self.Cpoints.append(np.zeros((3,4))) self.Caxes.append(np.zeros((3,3)))
def main(args):
# NOTE: Please set a random seed for your random joint generator so we can get the same path as you if we run your code!
np.random.seed(0)
deg_to_rad = np.pi / 180.
#Initialize robot object
mybot = Locobot.Locobot()
#Create environment obstacles
pointsObs = []
axesObs = []
envpoints, envaxes = rt.BlockDesc2Points(
rt.rpyxyz2H([0, 0., 0.], [0.275, -0.15, 0.]), [0.1, 0.1, 1.05])
pointsObs.append(envpoints), axesObs.append(envaxes)
envpoints, envaxes = rt.BlockDesc2Points(
rt.rpyxyz2H([0, 0., 0.], [-0.1, 0.0, 0.675]), [0.45, 0.15, 0.1])
pointsObs.append(envpoints), axesObs.append(envaxes)
envpoints, envaxes = rt.BlockDesc2Points(
rt.rpyxyz2H([0, 0., 0.], [-0.275, 0.0, 0.]), [0.1, 1.0, 1.25])
pointsObs.append(envpoints), axesObs.append(envaxes)
# base and mount
envpoints, envaxes = rt.BlockDesc2Points(
rt.rpyxyz2H([0, 0., 0.], [0., 0.0, 0.05996]),
[0.35004, 0.3521, 0.12276])
pointsObs.append(envpoints), axesObs.append(envaxes)
envpoints, envaxes = rt.BlockDesc2Points(
rt.rpyxyz2H([0, 0., 0.], [-0.03768, 0.0, 0.36142]),
[0.12001, 0.26, 0.5])
pointsObs.append(envpoints), axesObs.append(envaxes)
# Define initial pose
qInit = [-80. * deg_to_rad, 0., 0., 0., 0.]
# target box to grasp (You may only need the dimensions and pose, not the points and axes depending on your implementation)
target_coords, target_orientation = [0.5, 0.0, 0.05], [0, 0., 0.]
targetpoints, targetaxes = rt.BlockDesc2Points(
rt.rpyxyz2H(target_orientation, target_coords), [0.04, 0.04, 0.1])
#Generate query for a block object (note random sampling in TGoal)
QGoal = []
num_grasp_points = 5 # You can adjust the number of grasp points you want to sample here
while len(QGoal) < num_grasp_points:
# TODO: Sample grasp points here, get their joint configurations, and check if they are valid
TGoal = rt.rpyxyz2H(target_orientation,
[0.5, 0.0, 0.05 + np.random.uniform(-0.05, 0.05)])
ang, Err = mybot.IterInvKin(qInit, TGoal)
if mybot.RobotInLimits(ang) and np.linalg.norm(
Err[0:3]) < 0.005 and np.linalg.norm(Err[4:6]) < 0.005:
if not mybot.DetectCollision(ang, pointsObs, axesObs):
QGoal.append(np.array(ang))
#Create RRT graph to find path to a goal configuration
rrtVertices = []
rrtEdges = []
# initialize with initial joint configuration and parent
rrtVertices.append(qInit)
rrtEdges.append(0)
# Change these two hyperparameters as needed
thresh = 1.5
num_samples = 3000
FoundSolution = False
while len(rrtVertices) < num_samples and not FoundSolution:
# TODO: Implement your RRT planner here
# Use your sampler and collision detection from homework 2
# NOTE: Remember to add a goal bias when you sample
qRand = mybot.SampleRobotConfig(
) if np.random.uniform() >= 0.05 else QGoal[np.random.randint(
len(QGoal))]
idNear = FindNearest(rrtVertices, qRand)
qNear = rrtVertices[idNear]
if np.linalg.norm(np.array(qRand) - np.array(qNear)) > thresh:
qConnect = np.array(qNear) + thresh * (np.array(qRand) - np.array(
qNear)) / np.linalg.norm(np.array(qRand) - np.array(qNear))
else:
qConnect = qRand
if not mybot.DetectCollisionEdge(qConnect, qNear, pointsObs, axesObs):
rrtVertices.append(qConnect)
rrtEdges.append(idNear)
for qGoal in QGoal:
idNear = FindNearest(rrtVertices, qGoal)
if np.linalg.norm(qGoal - rrtVertices[idNear]) < 0.025:
rrtVertices.append(qGoal)
rrtEdges.append(idNear)
FoundSolution = True
break
if FoundSolution:
# Extract path - TODO: add your path from your RRT after a solution has been found
# Last in First Out
plan = []
plan.insert(0, np.array(rrtVertices[-1]))
edge = rrtEdges[-1]
while edge != 0:
idNear = FindNearest(rrtVertices, rrtVertices[edge])
plan.insert(0, np.array(rrtVertices[idNear]))
edge = rrtEdges[idNear]
plan.insert(0, np.array(qInit))
pointsObs.append(targetpoints), axesObs.append(targetaxes)
# Path shortening - TODO: implement path shortening in the for loop below
num_iterations = 1500 # change this hyperparameter as needed
for i in range(num_iterations):
anchorA = np.random.randint(len(plan) - 2)
anchorB = np.random.randint(anchorA + 1, len(plan) - 1)
shiftA = np.random.uniform(0, 1)
shiftB = np.random.uniform(0, 1)
candidateA = (1 - shiftA) * plan[anchorA] + shiftA * plan[anchorA +
1]
candidateB = (1 - shiftB) * plan[anchorB] + shiftB * plan[anchorB +
1]
if not mybot.DetectCollisionEdge(candidateA, candidateB, pointsObs,
axesObs):
while anchorB - anchorA:
plan.pop(anchorB)
anchorB = anchorB - 1
plan.insert(anchorA + 1, candidateB)
plan.insert(anchorA + 1, candidateA)
if args.use_pyrobot:
# Vizualize your plan in PyRobot
common_config = {}
common_config["scene_path"] = join(
dirname(abspath(__file__)), "../scene/locobot_motion_hw3.ttt")
robot = Robot("vrep_locobot", common_config=common_config)
# Execute plan
for q in plan:
robot.arm.set_joint_positions(q)
# grasp block
robot.gripper.close()
else:
# Visualize your Plan in matplotlib
for q in plan:
mybot.PlotCollisionBlockPoints(q, pointsObs)
else:
print("No solution found")
import pickle import RobotUtil as rt import time # NOTE: Please set a random seed for your random joint generator so we can get the same path as you if we run your code! random.seed(13) # np.random.seed(0) #Initialize robot object mybot=Locobot.Locobot() #Create environment obstacles pointsObs=[] axesObs=[] envpoints, envaxes = rt.BlockDesc2Points(rt.rpyxyz2H([0,0.,0.],[0.275,-0.15,0.]),[0.1,0.1,1.05]) pointsObs.append(envpoints), axesObs.append(envaxes) envpoints, envaxes = rt.BlockDesc2Points(rt.rpyxyz2H([0,0.,0.],[0.275,0.05,0.425]),[0.1,0.3,0.1]) pointsObs.append(envpoints), axesObs.append(envaxes) envpoints, envaxes = rt.BlockDesc2Points(rt.rpyxyz2H([0,0.,0.],[0.275,0.25,0.4]),[0.1,0.1,0.15]) pointsObs.append(envpoints), axesObs.append(envaxes) envpoints, envaxes = rt.BlockDesc2Points(rt.rpyxyz2H([0,0.,0.],[0.425,0.25,0.375]),[0.2,0.1,0.1]) pointsObs.append(envpoints), axesObs.append(envaxes) envpoints, envaxes = rt.BlockDesc2Points(rt.rpyxyz2H([0,0.,0.],[-0.1,0.0,0.675]),[0.45,0.15,0.1]) pointsObs.append(envpoints), axesObs.append(envaxes) envpoints, envaxes = rt.BlockDesc2Points(rt.rpyxyz2H([0,0.,0.],[-0.275,0.0,0.]),[0.1,1.0,1.25])
import RobotUtil as rt
reference_origin = (0, 0, 0)
reference_orientation = (0, 0, 0)
reference_dim = (3, 1, 2)
reference_H = rt.rpyxyz2H(reference_orientation, reference_origin)
reference_corners, reference_axes = rt.BlockDesc2Points(
reference_H, reference_dim)
test_cases = [{
'origin': (0, 1, 0),
'orient': (0, 0, 0),
'dim': (0.8, 0.8, 0.8)
}, {
'origin': (1.5, -1.5, 0),
'orient': (1, 0, 1.5),
'dim': (1, 3, 3)
}, {
'origin': (0, 0, -1),
'orient': (0, 0, 0),
'dim': (2, 3, 1)
}, {
'origin': (3, 0, 0),
'orient': (0, 0, 0),
'dim': (3, 1, 1)
}, {
'origin': (-1, 0, -2),
'orient': (.5, 0, .4),
'dim': (2, .7, 2)
}, {