diff --git a/ur5_control.py b/ur5_control.py index d4aa97e..00c2b12 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -283,7 +283,7 @@ def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_g def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): rob = robot.robot - joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset) + joints = get_joints_from_xyz_rel(robot, x, y, z, rx, ry, rz, l3offset=l3offset) # Return current positions if coordinates don't make sense if z<0: @@ -303,8 +303,8 @@ def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l return [o+j*i for j, o, i in zip(joints, offsets, inverse)] def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): - rob = robot.robot + start_joints = rob.getj() end_joint = get_joints_from_xyz_abs(robot, x, y, z, rx, ry, rz) @@ -336,7 +336,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=35, gripperlength=0.20+0.0 gripperx -= (1-math.cos(gripperangle)) * limb3 rz = math.pi / 2 # flip the whole wrist - return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz) + return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz) else: gripperangle = math.radians(gripperangle) @@ -384,8 +384,8 @@ def flip(robot): safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0] # Flip at safe position - rob.movej(offset_gripper_angle(*safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position - rob.movej(offset_gripper_angle(*safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper + rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position + rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper def safe_move(robot, x, y, z): rob = robot.robot @@ -408,8 +408,9 @@ if __name__ == "__main__": with open('config.yml', 'r') as fileread: #global config config = yaml.safe_load(fileread) - robot = Rob(config) - rob = robot.robot + robot = Rob(config) # robot of type Rob is the custom class above + rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously + print("Current tool pose is: ", rob.getl()) move_to_home(robot)