def __init__(self):
self.right_arm = Arm()
self.left_arm = Arm()
self.front_right_wheel = Wheel()
self.back_right_wheel = Wheel()
self.front_left_wheel = Wheel()
self.back_left_wheel = Wheel()
def __init__(self, simulation=True):
"""Initializes various aspects of the Fetch."""
rospy.init_node("fetch")
rospy.loginfo("initializing the Fetch...")
self.arm = Arm()
self.arm_joints = ArmJoints()
self.base = Base()
self.camera = RGBD()
self.head = Head()
self.gripper = Gripper(self.camera)
self.torso = Torso()
self.joint_reader = JointStateReader()
# Tucked arm starting joint angle configuration
self.names = ArmJoints().names()
self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0]
self.tucked_list = [(x,y) for (x,y) in zip(self.names, self.tucked)]
# Initial (x,y,yaw) position of the robot wrt map origin. We keep this
# fixed so that we can reset to this position as needed. The HSR's
# `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is
# the rotation about that axis (intuitively, the z axis). For the base,
# `base.odom` supplies both `position` and `orientation` attributes.
start = copy.deepcopy(self.base.odom.position)
yaw = Base._yaw_from_quaternion(self.base.odom.orientation)
self.start_pose = np.array([start.x, start.y, yaw])
rospy.loginfo("...finished initialization!")
def inchworm(seg_lens, q0, base=None):
"""Creates a complex arm with 10 segments
:param seg_lens: 1x10 vector of lengths of each sement
:type seg_lens: numpy.ndarray
:param q0: 1xN vector of the starting joint configuration
:type q0: numpy.ndarray
:param base: (Optional) optional (x, y, z) base location of arm
:type base: numpy.ndarray or None
:returns: "Noodle" arm
:rtype: maddux.robot.Arm
"""
# L1 = Link(0, seg_lens[0], 0, 1.571)
# L2 = Link(0, 0, seg_lens[1], 0)
# L3 = Link(0, 0, seg_lens[2], 0)
# L4 = Link(1.571, 0, seg_lens[3], 1.571)
L1 = Link(0, seg_lens[0], 0, 1.571)
L2 = Link(0, 0, seg_lens[1], 0)
L3 = Link(0, 0, seg_lens[2], 0)
L4 = Link(0, 0, 0, 1.571)
L4 = Link(0, 0, seg_lens[3], 0)
links = np.array([L1, L2, L3, L4])
robot = Arm(links, q0, 'inchworm', 10, base)
return robot
def setup():
print("finding Odrives")
odrives = odrive.find_any(find_multiple=3)
print("found Odrives")
# Multiple Odrive setup:
axis_dict = json.loads(open('axis_config.json', "r").read())
output_dict = {}
for od in odrives:
c = axis_dict[str(od.serial_number)]
output_dict[c['axis1']['name']] = Joint(od.axis1, c['axis1']['ratio'])
output_dict[c['axis0']['name']] = Joint(od.axis0, c['axis0']['ratio'])
arm_variables = {'D1': 3.319, 'D2': 3.125, 'A2': 7.913, 'A3': 9.0}
arm_dict = {}
arm_dict['right_arm'] = Arm(output_dict['1 lower'], output_dict['1 upper'],
output_dict['1 shoulder'], arm_variables)
#arm_dict['left_arm'] = Arm(output_dict['2 lower'], output_dict['2 upper'], output_dict['2 shoulder'], arm_variables)
for name, arm in arm_dict.items():
print(f"calibrating {name}")
arm.calibrate_arm()
print(f"homing {name}")
arm.home_arm()
print(f"enabling {name}")
arm.enable_arm()
print("Setup Complete!")
return arm_dict
def init_bandit(self, init_n_points):
# Initialize population
positions, fitnesses = self.init_selected_population(init_n_points)
# Esitmate number of clusters with silhouette_score
min_k = self.estimate_k_clusters(positions, self.max_n_clusters)
# Increase population until k <= min_k
population_step = int(init_n_points / 5)
# KMeans clustering
clusters_positions, clusters_fitnesses = self.k_means(
positions, fitnesses)
assert len(clusters_positions) == k
assert len(clusters_positions) == len(clusters_fitnesses)
# Initialize Arms
for positions, fitnesses in zip(clusters_positions,
clusters_fitnesses):
self.arms.append(Arm(self.obj, positions, fitnesses))
# Update projection matrix
for i in range(k):
positions_in = clusters_positions[i]
positions_out = []
for j in range(k):
if j != i:
positions_out.extend(clusters_positions[j])
self.arms[i].update_matrix(positions_in, clusters_fitnesses[i],
positions_out)
# Update population == n_points in each arm
self.resize_each_arm()
def __reset(self):
self.alphabet = [False] * 26
Framework.__init__(self)
Framework.setZoom(self, 115)
Framework.setCenter(self, [0, 1.74])
self.wristAngle = math.pi / 2
self.doorOpened = False
self.recording = False
self.reset = False
self.arm_angles = [
math.pi / 2, math.pi / 2, math.pi / 2, math.pi / 2, math.pi / 2
]
lengths = [1, 1, .4, .27]
self.arm = Arm(self.world,
self.arm_angles,
lengths,
basePos=(-.1, 0.08),
motorRange=2 * math.pi)
self.arm.target_pose[0] = random.uniform(-1, 1)
self.arm.target_pose[1] = random.uniform(1.5, 2.3)
ground = self.world.CreateBody(position=(0, 0))
ground.CreateEdgeChain([(-4, -2), (-4, .6), (-2, .6), (-2, 0), (2, 0),
(2, 6), (4, 6), (4, -2), (-2, -2)])
self.door = Door(self.world, (-2, .62), 1.2)
def simple_human_arm(seg1_len, seg2_len, q0, base=None):
"""Creates a simple human-like robotic arm with 7 links and 2 segments
with the desired lengths and starting joint configuration
:param seg1_len: The length of the first segment of the arm
:type seg1_len: int
:param seg2_len: The length of the second segment of the arm
:type seg2_len: int
:param q0: 1xN vector of the starting joint configuration
:type q0: numpy.ndarray
:param base: (Optional) (x, y, z) location of arm base
:type base: numpy.ndarray or None
:returns: 7 link, 2 segment "human" arm.
:rtype: maddux.robot.Arm
"""
L1 = Link(0, 0, 0, 1.571)
L2 = Link(0, 0, 0, -1.571)
L3 = Link(0, seg1_len, 0, -1.571)
L4 = Link(0, 0, seg2_len, -1.571)
L5 = Link(0, 0, 0, 1.571)
L6 = Link(0, 0, 0, 1.571)
L7 = Link(0, 0, 0, 0)
links = np.array([L1, L2, L3, L4, L5, L6, L7])
robot = Arm(links, q0, 'simple_human_arm', 4, base)
return robot
def world(self, n):
# mounting pts: x y z θ
w = World(*[(i * self.dx, 0, 0, 0) for i in range(0, n)])
# cmpts:
for i in range(0, n):
Arm("Arm" + str(i), w, i) #TODO option for Panda arm
return w
def as_arm(self):
arm = Arm([])
for i in range(int(len(self.genes) / 4)):
arm.rods.append(
Rod(self.genes[4 * i + 0], self.genes[4 * i + 1],
self.genes[4 * i + 2], self.genes[4 * i + 3]))
return arm
def noodle_arm(seg_lens, q0, base=None):
"""Creates a complex arm with 10 segments
:param seg_lens: 1x10 vector of lengths of each sement
:type seg_lens: numpy.ndarray
:param q0: 1xN vector of the starting joint configuration
:type q0: numpy.ndarray
:param base: (Optional) optional (x, y, z) base location of arm
:type base: numpy.ndarray or None
:returns: "Noodle" arm
:rtype: maddux.robot.Arm
"""
L1 = Link(0, seg_lens[0], 0, 1.571)
L2 = Link(0, seg_lens[1], 0, -1.571)
L3 = Link(0, seg_lens[2], 0, -1.571)
L4 = Link(0, seg_lens[3], 0, 1.571)
L5 = Link(0, seg_lens[4], 0, 1.571)
L6 = Link(0, seg_lens[5], 0, -1.571)
L7 = Link(0, seg_lens[6], 0, 1.571)
L8 = Link(0, seg_lens[7], 0, 1.571)
L9 = Link(0, seg_lens[8], 0, -1.571)
L10 = Link(0, seg_lens[9], 0, 1.571)
links = np.array([L1, L2, L3, L4, L5, L6, L7, L8, L9, L10])
robot = Arm(links, q0, 'noodle_arm', 10, base)
return robot
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.driver_stick = wpilib.Joystick(0)
self.codriver_stick = wpilib.Joystick(1)
self.left_shooter_motor = ctre.WPI_TalonSRX(0)
self.left_drivetrain_motor = ctre.WPI_TalonSRX(1)
self.right_drivetrain_motor = ctre.WPI_TalonSRX(2)
self.right_shooter_motor = ctre.WPI_TalonSRX(3)
self.intake_motor = wpilib.Spark(8)
self.arm_pivot_motor = wpilib.Spark(6)
self.arm_lock_motor = wpilib.Spark(5)
self.timer = wpilib.Timer()
#self.navx = navx.AHRS.create_spi()
self.duration = 20
self.drivetrain = DriveTrain(self.left_drivetrain_motor,
self.right_drivetrain_motor)
self.shooter = Shooter(self.intake_motor, self.left_shooter_motor,
self.right_shooter_motor)
self.arm = Arm(self.arm_pivot_motor, self.arm_lock_motor)
def __init__(self, config):
self.logging = True
self.gamepadConnected = False
self.gamepadToUse = 0
self.gamepad = None
#Create instance of Arm class
self.arm = Arm(config)
self.arm.resetArduino()
#Start Pygame
pygame.init()
pygame.joystick.init()
#Setup Variables for Buttons
self.buttonTotal = 10
self.lastButtonState = [0] * self.buttonTotal
#Setup Variables for Axis
self.axisTotal = 5
self.leftJoint = 0
self.rightJoint = 1
self.axisPositions = [0] * self.axisTotal
self.setSpeed = [0] * self.arm.joints
def main(argv):
rclpy.init(args=argv)
arm = Arm()
#collapse together
arm.move_joints(-0.0000, -0.1104, +1.5000, -1.3700, path_time=4.0)
arm.move_joints(joint1=-1.5000, path_time=1.0)
arm.move_joints(joint1=+1.5000, path_time=2.0)
arm.move_joints(joint1=-1.5000, path_time=2.0)
arm.move_joints(joint1=+1.5000, path_time=2.0)
arm.move_joints(joint1=-1.5000, path_time=2.0)
arm.move_joints(joint1=+1.5000, path_time=2.0)
arm.move_joints(joint1=-1.5000, path_time=2.0)
arm.move_joints(joint1=+1.5000, path_time=2.0)
arm.move_joints(joint1=+0.0000, path_time=2.0)
time.sleep(2.0)
# return to neutral
arm.move_joints(-0.1227, -1.0440, +0.6213, +0.4341, path_time=4.0)
arm.move_to(+0.1360, +0.0000, +0.2324, accelerate=0.5)
time.sleep(3.0)
# dizzy
arm.move_joints(joint4=+0.7000, path_time=1.0)
arm.circle(radius=0.010, path_time=2.0)
arm.circle(radius=0.020, path_time=4.0)
arm.circle(radius=0.010, path_time=2.0)
# return to neutral
arm.move_to(+0.1360, +0.0000, +0.2324, accelerate=0.3)
def main (argv):
rclpy.init (args=argv)
arm = Arm()
#stretch up
arm.move_joints (-0.000, +0.000, -1.6580, -0.0000, path_time=4.0)
time.sleep (2.0)
arm.move_joints (-0.1227, -1.0440, +0.6213, +0.4341, path_time=4.0)
time.sleep (2.0)
#stretch forward
arm.move_joints (+0.0000, -1.5900 +0.2800, +1.5000, path_time=4.0)
arm.move_joints (+0.0000, +1.4650, -1.6337, +0.2378, path_time=6.0)
time.sleep (2.0)
arm.move_joints (+0.0000, -1.5900, +0.2800, +1.5000, path_time=4.0)
time.sleep (2.0)
#stretch left
arm.move_joints (-1.5447, -1.5900, +0.2800, +1.5000, path_time=4.0)
arm.move_joints (-1.5447, +1.4650, -1.6337, +0.2378, path_time=6.0)
time.sleep (2.0)
arm.move_joints (-1.5447, -1.5900, +0.2800, +1.5000, path_time=4.0)
time.sleep (2.0)
#stretch right
arm.move_joints (+1.5447, -1.5900, +0.2800, +1.5000, path_time=4.0)
arm.move_joints (+1.5447, +1.4650, -1.6337, +0.2378, path_time=6.0)
time.sleep (2.0)
arm.move_joints (+1.5447, -1.5900, +0.2800, +1.5000, path_time=4.0)
time.sleep (2.0)
# return to neutral
arm.move_joints (-0.1227, -1.0440, +0.6213, +0.4341, path_time=4.0)
arm.move_to (+0.1360, +0.0000, +0.2324, accelerate = 0.5)
def __init__(self, simulation=True):
"""Initializes various aspects of the Fetch.
TODOs: get things working, also use `simulation` flag to change ROS
topic names if necessary (especially for the cameras!). UPDATE: actually
I don't think this is necessary now, they have the same topic names.
"""
rospy.init_node("fetch")
self.arm = Arm()
self.arm_joints = ArmJoints()
self.base = Base()
self.camera = RGBD()
self.head = Head()
self.gripper = Gripper(self.camera)
self.torso = Torso()
self.joint_reader = JointStateReader()
# Tucked arm starting joint angle configuration
self.names = ArmJoints().names()
self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0]
self.tucked_list = [(x, y) for (x, y) in zip(self.names, self.tucked)]
# Initial (x,y,yaw) position of the robot wrt map origin. We keep this
# fixed so that we can reset to this position as needed. The HSR's
# `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is
# the rotation about that axis (intuitively, the z axis). For the base,
# `base.odom` supplies both `position` and `orientation` attributes.
start = copy.deepcopy(self.base.odom.position)
yaw = Base._yaw_from_quaternion(self.base.odom.orientation)
self.start_pose = np.array([start.x, start.y, yaw])
self.TURN_SPEED = 0.3
self.num_restarts = 0
def __init__(self, arm_n, n, e):
self.arm_n = arm_n
self.n = n
self.e = e
self.arms = [Arm(math.fabs(random.random()), 1) for _ in range(arm_n)]
self.counts = [1 for _ in range(arm_n)]
self.rewards = [1.0 for _ in range(arm_n)]
self.search_n = 0
def get_arms(arm_count, tape_size):
true_means = rvh.get_uniform_sample(0, 1, arm_count)
arms = []
for i in range(arm_count):
arm = Arm(true_means[i], size=tape_size)
arms.append(arm)
return true_means, arms
def __init__(self):
self.arm = Arm(2)
self.pos_ini = [
2000, 2500, 500, 2500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,
1500, 1000, 2500, 500, 2500, 1500, 1500, 1500, 1500, 1500, 1500,
1500, 1500
]
pass
def world(x, y, z, a, b, c):
# mounting pts: x y z θ comp
w = World(
(0, 0, 0, 0), # arm
(0, 0, 0, 0)) # point
#arm = Arm("arm", w, 0, -2.2689280275926285, 2.2689280275926285, 0) #folded toward the back
arm = Arm("arm", w, 0, a, b, c)
point = PointProcess("point", x, y, z, w, 1)
return w
def __init__(self):
# create a RobotCommander
self.robot = RobotCommander()
# create a PlanningScene Interface object
self.scene = PlanningSceneInterface()
# create left arm move group and gripper
self.left_arm = Arm("left_arm")
self.left_gripper = self.left_arm.gripper = Gripper("left")
# create right arm move group and gripper
self.right_arm = Arm("right_arm")
self.right_gripper = self.right_arm.gripper = Gripper("right")
self.gripper_offset = 0.0
# reset robot
self.reset()
def main(argv):
rclpy.init(args=argv)
arm = Arm()
while True:
print ("home")
arm.move_to (+0.1370, +0.0000, +0.2315)
print ("sleep")
time.sleep (random.randrange (5, 30) + random.randrange (5, 30))
random_call (arm)
def __init__ (self):
rospy.Subscriber("/head_camera/rgb/image_raw",Image,self.RGBImageCallback)
rospy.Subscriber("/head_camera/depth_registered/image_raw",Image,self.DepthImageCallback)
rospy.Subscriber("/ar_pose_marker",AlvarMarkers,self.GetArPosesCallBack)
self.bridge = CvBridge()
self.Arm = Arm()
self.Gripper = Gripper()
self.Head = Head()
self.PoseProcessing = PoseProcessing()
def main():
"Function to contsruct an arm and input movements"
parts = [
Joint(name='r1', z_axis=[0, 0, 1]),
Bone(length=5),
Joint(name='r2', z_axis=[0, 1, 0])
]
robot_arm = Arm(parts=parts)
def unload(self):
print("unload")
self.stop()
# os.system("sudo python unload.py")
# pass
b = Arm()
b.step10()
b.step11()
b.step12()
b.step14()
b.step15()
def capture(self):
print("capture")
self.stop()
b = Arm()
b.step2()
b.step3()
b.step4()
b.step5()
b.step6()
b.step7()
b.step8()
def __init__(self):
# create a RobotCommander
self.robot = RobotCommander()
# create a PlanningSceneInterface object
self.scene = PlanningSceneInterface()
# create arm
self.arm = Arm("manipulator")
# create gripper
self.gripper = Gripper()
def test_folded_above_0(self, debug=False):
# mounting pts: x y z θ comp
w = World(
(0, 0, 1, 0), # arm
(0, 0, 0, 0)) # point
arm = Arm("arm", w, 0, -2.2689280275926285, -2.2689280275926285,
0) #folded toward the back
below = FpProcess(
"point", self.below(0.97), w,
1) #The folded arm FP extends 2-3cm below 0 due to maxFP
ch = choreo1()
self.assertTrue(self.check0(ch, w, debug))
def test_arm(self):
arm = Arm(0.3)
rewards = []
T = 20
for t in range(1, T + 1):
reward = arm.pull()
rewards.append(reward)
total = sum(rewards)
assert arm.pull_count == 20
def __init__(self, torso_center, lower_body_flag):
self._body_center = torso_center
self._lower_body_flag = lower_body_flag
self.head = Head(
Point(torso_center.x, torso_center.y + 15, torso_center.z))
self.torso = Torso(
Point(torso_center.x, torso_center.y, torso_center.z))
self.left_arm = Arm(
Point(torso_center.x + 6.6, torso_center.y + 8, torso_center.z))
self.right_arm = Arm(
Point(torso_center.x - 6.6, torso_center.y + 8, torso_center.z))
self.right_arm.set_slocal_rotate_angle(180, 0, 0)
if self._lower_body_flag:
self.left_leg = Leg(
Point(torso_center.x + 4, torso_center.y - 10.6,
torso_center.z))
self.right_leg = Leg(
Point(torso_center.x - 4, torso_center.y - 10.6,
torso_center.z))
self.right_leg.set_hlocal_rotate_angle(180, 0, 0)
def __init__(self, limb_name):
self.limb_name = limb_name
self.arm = Arm(limb_name)
self.available_tread = True
if limb_name == 'left':
self.other_limb_name = 'right'
else:
self.other_limb_name = 'left'
self.otherArm = Arm(self.other_limb_name)
#self.otherArm.tuck_arm()
self.cards = np.array([])
self.bridge = CvBridge()
self.cards = Cards()
#rospy.init_node(limb_name, anonymous=True)
self.display_pub = rospy.Publisher('/robot/xdisplay', Image)
head_camera = baxter_interface.CameraController('head_camera')
head_camera.close()
self.other_camera = baxter_interface.CameraController(
self.other_limb_name + '_hand_camera')
self.other_camera.close()
self.camera = baxter_interface.CameraController(self.limb_name +
'_hand_camera')
self.camera.open()
self.arm.calibrate()
resp = 'n'
while resp is not 'y':
resp = str(
raw_input(
'Please place the gripper arm at the center of the card holder, press "y" to continue: '
))
self.arm.go_home()
if self.available_tread:
cam_thread = threading.Thread(target=self.get_camera)
choices_thread = threading.Thread(target=self.pick_choices)
cam_thread.start()
choices_thread.start()
rospy.spin()
cv2.destroyAllWindows()
