def main():
arm = InterbotixManipulatorXS("wx200", "arm", "gripper")
arm.gripper.close(2.0)
arm.gripper.open(2.0)
arm.gripper.set_pressure(1.0)
arm.gripper.close(2.0)
arm.gripper.open(2.0)
def main():
joint_positions = [-1.0, 0.5, 0.5, 0, -0.5, 1.57]
bot = InterbotixManipulatorXS("wx250s", "arm", "gripper")
bot.arm.go_to_home_pose()
bot.arm.set_joint_positions(joint_positions)
bot.arm.go_to_home_pose()
bot.arm.go_to_sleep_pose()
def main():
bot = InterbotixManipulatorXS("wx250", "arm", "gripper")
bot.arm.go_to_home_pose()
bot.arm.set_ee_cartesian_trajectory(z=-0.2)
bot.arm.set_ee_cartesian_trajectory(x=-0.2)
bot.arm.set_ee_cartesian_trajectory(z=0.2)
bot.arm.set_ee_cartesian_trajectory(x=0.2)
bot.arm.go_to_sleep_pose()
def main():
# Initialize the arm module along with the pointcloud and armtag modules
bot = InterbotixManipulatorXS("wx250s")
bot.dxl.robot_set_motor_registers("single", "shoulder", "Position_P_Gain",
1500)
bot.dxl.robot_set_motor_registers("single", "elbow", "Position_P_Gain",
1500)
bot.dxl.robot_set_motor_registers("single", "waist", "Position_P_Gain",
1500)
pcl = InterbotixPointCloudInterface()
armtag = InterbotixArmTagInterface()
# set initial arm and gripper pose
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.gripper.open()
# get the ArmTag pose
bot.arm.set_ee_pose_components(y=0.3, z=0.2)
time.sleep(0.5)
armtag.find_ref_to_arm_base_transform()
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
# get the cluster positions
# sort them from max to min 'y' position w.r.t. the 'wx250s/base_link' frame
success, clusters = pcl.get_cluster_positions(ref_frame="wx250s/base_link",
sort_axis="y",
reverse=True)
color_x_dict = {}
# pick up all the objects and sort them into two different lines depending on their color
for cluster in clusters:
x, y, z = cluster["position"]
z_dist = 0.2 - (z + 0.02)
bot.arm.set_ee_pose_components(x=x, y=y, z=0.2, pitch=0.7)
bot.arm.set_ee_cartesian_trajectory(z=-z_dist, y=0.01)
bot.gripper.close()
bot.arm.set_ee_cartesian_trajectory(z=z_dist, y=-0.01)
clr = color_compare(cluster["color"])
if clr not in color_x_dict:
color_x_dict[clr] = -0.14
else:
color_x_dict[clr] += 0.07
if (clr == "yellow"):
y = -0.25
elif (clr == "blue"):
y = -0.35
bot.arm.set_ee_pose_components(x=color_x_dict[clr],
y=y,
z=0.2,
pitch=0.7,
yaw=-math.pi / 2.0)
bot.arm.set_ee_cartesian_trajectory(z=-z_dist)
bot.gripper.open()
bot.arm.set_ee_cartesian_trajectory(z=z_dist)
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.arm.go_to_sleep_pose()
def main():
T_sd = np.identity(4)
T_sd[0,3] = 0.3
T_sd[1,3] = 0
T_sd[2,3] = 0.2
bot = InterbotixManipulatorXS("wx250s", "arm", "gripper")
bot.arm.go_to_home_pose()
bot.arm.set_ee_pose_matrix(T_sd)
bot.arm.go_to_home_pose()
bot.arm.go_to_sleep_pose()
def main():
joint_positions1 = [0, 0, 0.506, -0.531, 0]
joint_positions2 = [0, 0, 0.506, -0.531, -1.57]
bot = InterbotixManipulatorXS("rx150", "arm", "gripper")
bot.arm.go_to_home_pose()
bot.arm.set_joint_positions(joint_positions1)
rospy.sleep(5)
bot.arm.set_joint_positions(joint_positions2)
rospy.sleep(8)
bot.arm.go_to_home_pose()
bot.arm.go_to_sleep_pose()
def robot_2():
robot_2 = InterbotixManipulatorXS(robot_model="wx200",
robot_name="arm_2",
moving_time=1.0,
gripper_pressure=1.0,
init_node=False)
robot_2.arm.set_ee_pose_components(x=0.3, z=0.2)
robot_2.gripper.open(delay=0.05)
robot_2.arm.set_single_joint_position("waist", -math.pi / 4.0)
robot_2.gripper.close(delay=0.05)
robot_2.arm.set_single_joint_position("waist", 0)
robot_2.arm.go_to_sleep_pose()
def main():
# Initialize the arm module along with the pointcloud and armtag modules
bot = InterbotixManipulatorXS("wx250s")
pcl = InterbotixPointCloudInterface()
armtag = InterbotixArmTagInterface()
# set the initial arm and gripper pose
bot.dxl.robot_set_motor_registers("single", "waist", "Position_P_Gain",
1500)
bot.dxl.robot_set_motor_registers("single", "shoulder", "Position_P_Gain",
1500)
bot.dxl.robot_set_motor_registers("single", "elbow", "Position_P_Gain",
1500)
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.gripper.open()
# get the ArmTag pose
bot.arm.set_ee_pose_components(y=0.3, z=0.2)
time.sleep(0.5)
armtag.find_ref_to_arm_base_transform()
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
# get the cluster positions
# sort them from min to max 'y' position w.r.t. the 'wx250s/base_link' frame
success, clusters = pcl.get_cluster_positions(ref_frame="wx250s/base_link",
sort_axis="y")
# sort clusters by their size - from most points to min points
sorted_clusters = sorted(clusters,
key=lambda cluster: cluster["num_points"],
reverse=True)
# pick up all the objects and place in a single row based on size
x_des = -0.07
for cluster in sorted_clusters:
x, y, z = cluster["position"]
z_dist = 0.2 - (z + 0.01)
bot.arm.set_ee_pose_components(x=x, y=y, z=0.2, pitch=0.5)
bot.arm.set_ee_cartesian_trajectory(z=-z_dist)
bot.gripper.close()
bot.arm.set_ee_cartesian_trajectory(z=z_dist)
bot.arm.set_ee_pose_components(x=x_des,
y=-0.3,
z=0.2,
pitch=0.5,
yaw=-math.pi / 2)
bot.arm.set_ee_cartesian_trajectory(z=-z_dist)
bot.gripper.open()
bot.arm.set_ee_cartesian_trajectory(z=z_dist)
x_des += 0.07
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.arm.go_to_sleep_pose()
def main():
# Initialize the arm module along with the pointcloud and armtag modules
bot = InterbotixManipulatorXS("wx200", moving_time=1.5, accel_time=0.75)
pcl = InterbotixPointCloudInterface()
armtag = InterbotixArmTagInterface()
# set initial arm and gripper pose
bot.dxl.robot_set_motor_registers("single", "shoulder", "Position_P_Gain",
1500)
bot.dxl.robot_set_motor_registers("single", "elbow", "Position_P_Gain",
1500)
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.gripper.open()
# get the ArmTag pose
bot.arm.set_ee_pose_components(y=0.3, z=0.2)
time.sleep(0.5)
armtag.find_ref_to_arm_base_transform()
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
# get the cluster positions
# sort them from max to min 'x' position w.r.t. the 'wx200/base_link' frame
success, clusters = pcl.get_cluster_positions(ref_frame="wx200/base_link",
sort_axis="y",
reverse=True)
# pick up all the objects and drop them in baskets
for cluster in clusters:
x, y, z = cluster["position"]
bot.arm.set_ee_pose_components(x=x, y=y, z=0.1, pitch=0.5)
bot.arm.set_ee_pose_components(x=x, y=y, z=z + 0.02, pitch=0.5)
bot.gripper.close()
bot.arm.set_ee_pose_components(x=x, y=y, z=0.1, pitch=0.5)
clr = color_compare(cluster["color"])
if (clr == "red"):
bot.arm.set_ee_pose_components(x=0.24, y=-0.1, z=0.2)
elif (clr == "yellow"):
bot.arm.set_ee_pose_components(x=0.38, y=-0.24, z=0.2)
elif (clr == "purple"):
bot.arm.set_ee_pose_components(x=0.24, y=-0.24, z=0.2)
elif (clr == "blue"):
bot.arm.set_ee_pose_components(x=0.38, y=-0.1, z=0.2)
else:
# if color cannot be recognized, then put the block back...
bot.arm.set_ee_pose_components(x=x, y=y, z=z + 0.01, pitch=0.5)
bot.gripper.open()
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.arm.go_to_sleep_pose()
def main():
bot = InterbotixManipulatorXS("wx250", "arm", "gripper")
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.arm.set_single_joint_position("waist", np.pi/2.0)
bot.gripper.open()
bot.arm.set_ee_cartesian_trajectory(x=0.1, z=-0.16)
bot.gripper.close()
bot.arm.set_ee_cartesian_trajectory(x=-0.1, z=0.16)
bot.arm.set_single_joint_position("waist", -np.pi/2.0)
bot.arm.set_ee_cartesian_trajectory(pitch=1.5)
bot.arm.set_ee_cartesian_trajectory(pitch=-1.5)
bot.arm.set_single_joint_position("waist", np.pi/2.0)
bot.arm.set_ee_cartesian_trajectory(x=0.1, z=-0.16)
bot.gripper.open()
bot.arm.set_ee_cartesian_trajectory(x=-0.1, z=0.16)
bot.arm.go_to_home_pose()
bot.arm.go_to_sleep_pose()
def main():
# Initialize the arm module along with the pointcloud and armtag modules
bot = InterbotixManipulatorXS("vx300s", moving_time=1.5, accel_time=0.75, gripper_pressure=1.0)
pcl = InterbotixPointCloudInterface()
armtag = InterbotixArmTagInterface()
# get the ArmTag pose
bot.arm.set_ee_pose_components(x=0.5, z=0.2)
bot.arm.set_ee_pose_components(x=0.5, y=-0.1, z=0.2)
time.sleep(0.5)
armtag.find_ref_to_arm_base_transform()
bot.arm.set_ee_pose_components(x=0.5, z=0.2)
bot.arm.go_to_sleep_pose()
# get the cluster positions
# sort them from min to max 'x' position w.r.t. the 'vx300s/base_link' frame
success, clusters = pcl.get_cluster_positions(ref_frame="vx300s/base_link", sort_axis="x", reverse=False)
# set initial arm and gripper pose
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.gripper.open()
# pick up all the chess pieces and place them in the right drawer based on color
for cluster in clusters:
x, y, z = cluster["position"]
bot.arm.set_ee_pose_components(x=x, y=y, z=0.1, yaw=0)
bot.arm.set_ee_pose_components(x=x, y=y, z=z+0.02, yaw=0)
bot.gripper.close()
bot.arm.set_ee_pose_components(x=x, y=y, z=0.1, yaw=0)
clr = color_compare(cluster["color"])
if (clr == "white"):
bot.arm.set_ee_pose_components(x=0.5, y=-0.33, z=0.1, yaw=0)
elif (clr == "black"):
bot.arm.set_ee_pose_components(x=0.5, y=0.33, z=0.1, yaw=0)
bot.gripper.open()
bot.arm.set_ee_pose_components(x=0.3, z=0.2)
bot.arm.go_to_sleep_pose()
def main():
bot = InterbotixManipulatorXS("wx250", "arm", "gripper")
bot.arm.go_to_home_pose()
bot.arm.set_ee_pose_components(x=0.2, y=0.1, z=0.2, roll=1.0, pitch=1.5)
bot.arm.go_to_home_pose()
bot.arm.go_to_sleep_pose()
def check_load(joint_states):
global jointLoad
jointLoad = joint_states.effort[3]
def process_state(State):
print("Load: " + str(State.load) + " State: " + State.state)
if not (State.load <= -115 and State.load >= -134.5):
print("Load not initialised correctly")
reset_load = [0, -1.57, 0, -0.531, 0]
bot.arm.set_joint_positions(reset_load)
rospy.sleep(1)
neutral_joint_position = [0, 0, 0.506, -0.531, 0]
bot.arm.set_joint_positions(neutral_joint_position)
else:
print("Ready to receive cup")
return State
if __name__ == '__main__':
bot = InterbotixManipulatorXS("rx150", "arm", "gripper")
set_wrist_pose()
listener()
robot_arm = State_machine()
while not rospy.is_shutdown():
rospy.sleep(0.05)
robot_arm.load = jointLoad
robot_arm = process_state(robot_arm)
def main():
joint_currents = [0, 200, 200, 50, 0]
bot = InterbotixManipulatorXS(robot_model="vx250")
bot.dxl.robot_set_operating_modes("group", "arm", "current")
bot.dxl.robot_write_commands("arm", joint_currents)
