def main():
joint_positions = [-1.0, 0.5, 0.5, 0, -0.5, 1.57]
arm = InterbotixRobot(robot_name="wx250s", mrd=mrd)
arm.go_to_home_pose()
arm.set_joint_positions(joint_positions)
arm.go_to_home_pose()
arm.go_to_sleep_pose()
def main():
joint_positions = [0, 0]
turret = InterbotixRobot(robot_name="pxxls", moving_time=2, accel_time=0.3)
turret.set_joint_commands(joint_positions, delay=2.0)
turret.set_single_joint_command("pan", 1.57, delay=2.0)
turret.set_joint_commands([3.14, 1.5], delay=2.0)
turret.set_joint_commands([1, 1], delay=2.0)
def main():
arm = InterbotixRobot(robot_name="wx250", mrd=mrd)
arm.go_to_home_pose()
arm.set_ee_cartesian_trajectory(z=-0.2)
arm.set_ee_cartesian_trajectory(x=-0.2)
arm.set_ee_cartesian_trajectory(z=0.2)
arm.set_ee_cartesian_trajectory(x=0.2)
arm.go_to_sleep_pose()
def main():
T_sd = np.identity(4)
T_sd[0,3] = 0.3
T_sd[1,3] = 0.1
T_sd[2,3] = 0.2
arm = InterbotixRobot(robot_name="wx250s", mrd=mrd)
arm.go_to_home_pose()
arm.set_ee_pose_matrix(T_sd)
arm.go_to_home_pose()
arm.go_to_sleep_pose()
def main():
joint_positions = [0, 0]
turret = InterbotixRobot(robot_name="pxxls",
moving_time=131,
accel_time=15,
use_time=False)
turret.set_joint_commands(joint_positions, delay=2.0)
turret.set_single_joint_command("pan", 1.57, delay=1.0)
turret.set_joint_commands([3.14, 1.5],
moving_time=20,
accel_time=5,
delay=5)
turret.set_joint_commands([1, 1], delay=5)
def __init__(self):
self.arm = InterbotixRobot(robot_name="rx150",
mrd=mrd,
gripper_pressure=1)
self.pt_sub = rospy.Subscriber('/detections/real_center_base_link',
PointStamped,
self.callback,
queue_size=1)
self.relay_pub = rospy.Publisher('/vacuum', String, queue_size=1)
self.request_detection_pub = rospy.Publisher('/robot/get_new_point',
String,
queue_size=1)
self.target = Point()
self.target.x = rospy.get_param("~target_x")
self.target.y = rospy.get_param("~target_y")
self.target.z = rospy.get_param("~target_z")
self.pickup_z = rospy.get_param("~pickup_z")
self.detect_mode = True
def main():
arm = InterbotixRobot(robot_name="wx250s", mrd=mrd)
arm.set_ee_pose_components(x=0.3, z=0.2)
arm.set_single_joint_position("waist", np.pi / 2.0)
arm.open_gripper()
arm.set_ee_cartesian_trajectory(x=0.1, z=-0.16)
arm.close_gripper()
arm.set_ee_cartesian_trajectory(x=-0.1, z=0.16)
arm.set_single_joint_position("waist", -np.pi / 2.0)
arm.set_ee_cartesian_trajectory(pitch=1.5)
arm.set_ee_cartesian_trajectory(pitch=-1.5)
arm.set_single_joint_position("waist", np.pi / 2.0)
arm.set_ee_cartesian_trajectory(x=0.1, z=-0.16)
arm.open_gripper()
arm.set_ee_cartesian_trajectory(x=-0.1, z=0.16)
arm.go_to_home_pose()
arm.go_to_sleep_pose()
def main():
joint_currents = [0, 200, 200, 50, 0]
arm = InterbotixRobot(robot_name="vx250", mrd=mrd)
arm.set_joint_operating_mode("current")
arm.set_joint_commands(joint_currents)
from interbotix_sdk.robot_manipulation import InterbotixRobot
def convert_to_cyl(position):
x = -position[0] - 0.30 # distance of base from camera - x axis
y = -position[1]
z = position[2] - 0.25 # distance of base from camera - z axis
r = math.sqrt(x**2 + z**2)
theta = math.atan2(x, -z) # - math.pi/2
return (r, theta, y)
position = [0.041852131485939026, 0.015240330249071121, 0.1990000158548355]
converted_position = convert_to_cyl(position)
arm = InterbotixRobot(robot_name="px100", mrd=mrd)
arm.go_to_home_pose()
arm_position = (0.0, 0.0)
arm.open_gripper(2.0)
arm.set_ee_pose_components(x=converted_position[0],
z=converted_position[2] + .2,
blocking=True)
arm.set_ee_cartesian_trajectory(x=-0.05)
arm.set_single_joint_position("waist", converted_position[1], blocking=True)
arm.set_ee_cartesian_trajectory(x=+0.05)
arm.close_gripper(2.0)
arm.set_ee_cartesian_trajectory(x=-0.05)
arm.go_to_home_pose()
arm.open_gripper(2.0)
arm.go_to_sleep_pose()
def main():
arm = InterbotixRobot(robot_name="vx300s", mrd=mrd)
arm.close_gripper(2.0)
arm.open_gripper(2.0)
arm.set_gripper_pressure(1.0)
arm.close_gripper(2.0)
arm.open_gripper(2.0)
from interbotix_sdk.robot_manipulation import InterbotixRobot
from interbotix_descriptions import interbotix_mr_descriptions as mrd
import time
# The robot object is what you use to control the robot
robot = InterbotixRobot(robot_name="px100", mrd=mrd)
mode = 'h'
# Let the user select the position
# robot.open_gripper(2.0)
# time.sleep(1)
# robot.close_gripper(2.0)
robot.go_to_home_pose()
robot.set_single_joint_position("wrist", -45)
robot.set_single_joint_position("wrist", 45)
robot.set_single_joint_position("elbow", 0.5)
# robot.set_single_joint_position("elbow",-1)
# robot.set_single_joint_position("shoulder",1)
# robot.set_single_joint_position("shoulder",-1)
print("------")
robot.go_to_home_pose()
print(robot.get_joint_positions())
robot.go_to_sleep_pose()
from interbotix_sdk.robot_manipulation import InterbotixRobot
from interbotix_descriptions import interbotix_mr_descriptions as mrd
# The robot object is what you use to control the robot
robot = InterbotixRobot(robot_name="px100", mrd=mrd, gripper_pressure=1.0)
mode = 'h'
# Let the user select the position
robot.go_to_home_pose()
robot.go_to_sleep_pose()
robot.go_to_home_pose()
robot.set_gripper_pressure(1.3)
while mode != 'q':
mode = input("[h]ome, [s]leep, [q]uit ")
if mode == "h":
robot.go_to_home_pose()
elif mode == "s":
robot.go_to_sleep_pose()
elif mode == "c":
robot.close_gripper(3.0)
elif mode == "o":
robot.open_gripper(3.0)
def main():
arm = InterbotixRobot(robot_name="rx150", mrd=mrd)
arm.go_to_home_pose()
arm.set_joint_positions(jp[0])
arm.set_gripper_pressure(1.0)
arm.open_gripper()
arm.set_joint_positions(jp[1])
arm.close_gripper()
arm.go_to_home_pose()
arm.set_joint_positions(jp[2])
arm.open_gripper()
arm.set_joint_positions(jp[3])
arm.close_gripper()
arm.go_to_home_pose()
arm.set_joint_positions(jp[4])
# Import Numpy for easy array manipulation
import numpy as np
# Import OpenCV for easy image rendering
import cv2
from interbotix_sdk.robot_manipulation import InterbotixRobot
from interbotix_descriptions import interbotix_mr_descriptions as mrd
import time
# The robot object is what you use to control the robot
robot = InterbotixRobot(robot_name="px100", mrd=mrd)
title_window = "OpenCV Stream"
cv2.namedWindow(title_window, cv2.WINDOW_AUTOSIZE)
alpha_slider_max = 100
HueValU = 255
SatValU = 255
ValValU = 255
waist_value = 0
elbow_value = 0
shoulder_value = 0
def on_trackbarHD(val):
global HueValD
alpha = val / alpha_slider_max
beta = (1.0 - alpha)
HueValD = int(alpha * 255)
def on_trackbarSD(val):
class PickAndPlace:
def __init__(self):
self.arm = InterbotixRobot(robot_name="rx150",
mrd=mrd,
gripper_pressure=1)
self.pt_sub = rospy.Subscriber('/detections/real_center_base_link',
PointStamped,
self.callback,
queue_size=1)
self.relay_pub = rospy.Publisher('/vacuum', String, queue_size=1)
self.request_detection_pub = rospy.Publisher('/robot/get_new_point',
String,
queue_size=1)
self.target = Point()
self.target.x = rospy.get_param("~target_x")
self.target.y = rospy.get_param("~target_y")
self.target.z = rospy.get_param("~target_z")
self.pickup_z = rospy.get_param("~pickup_z")
self.detect_mode = True
def request_detection(self):
self.request_detection_pub.publish("foo")
def move_to_point(self, pt):
"""
Abstract the set_ee_pose_compenent method to clean up callback.
Args:
pt (Point) -- desired end effector point
"""
# List of hard coded positions that the IKSolver will use as seeds to
# better solve the planning problem
custom_guesses = [[
-0.012271846644580364, 0.7470486760139465, -0.26691266894340515,
0.6181942820549011, 0.08897088468074799
],
[
-0.6181942820549011, 1.0047574043273926,
0.4218447208404541, 0.11044661700725555,
0.0920388475060463
],
[
-1.121340036392212, 0.8620972037315369,
-0.04601942375302315, 0.7900001406669617,
0.006135923322290182
],
[
-1.7794177532196045, 0.5292233824729919,
-0.503145694732666, 0.9526020884513855,
-0.00920388475060463
]]
for guess in custom_guesses:
_, res = self.arm.set_ee_pose_components(x=pt.x,
y=pt.y,
z=pt.z,
custom_guess=guess)
if (res):
# Planning problem solved
return True
return False
def callback(self, msg):
"""
Main callback that moves the robot to the detected point, turns on the vacuum, moves to target
and then turns off the vacuum and returns home
"""
self.detect_mode = False
self.arm.go_to_home_pose()
pt = msg.point
# Hard code the z value so the vacuum head is appropriately placed
# above the lens
pt.z = 0.13
self.move_to_point(pt)
pt.z = self.pickup_z
self.move_to_point(pt)
self.turn_on_vacuum()
time.sleep(1.5)
pt.z = 0.2
self.move_to_point(pt)
self.arm.go_to_home_pose()
# Move to target
pt.x = self.target.x
pt.y = self.target.y
pt.z = 0.2
self.move_to_point(pt)
self.move_to_point(self.target)
pt.x = self.target.x
pt.y = self.target.y
pt.z = 0.2
time.sleep(1.0)
self.turn_off_vacuum()
time.sleep(0.5)
self.move_to_point(pt)
# Home
self.arm.go_to_home_pose()
self.arm.go_to_sleep_pose()
self.detect_mode = True
def turn_on_vacuum(self):
self.relay_pub.publish(String("ON"))
def turn_off_vacuum(self):
self.relay_pub.publish(String("OFF"))
def run(self):
if (self.detect_mode):
self.request_detection()
# use_gripper: True
# home_pos: [0.0, 0.0, 0.0, 0.0, 0.0]
# sleep_pos: [0.0, -1.8, -1.55, -0.8, 0.0]
# num_joints: 5
# num_single_joints: 6
# This script commands some arbitrary positions to the arm joints
#
# To get started, open a terminal and type 'roslaunch interbotix_sdk arm_run.launch robot_name:=wx250s use_time_based_profile:=true gripper_operating_mode:=pwm'
# Then change to this directory and type 'python joint_position_control.py'
movingTime = 2000.0
accelTime= 600.0
arm = InterbotixRobot(robot_name="rx150", mrd=mrd, moving_time=movingTime, accel_time=accelTime, gripper_pressure=1, use_time=False)
arm.go_to_home_pose()
rospy.sleep(movingTime/1000)
dest = arm.get_joint_positions()
waist = dest[0]
shoulder= dest[1]
elbow = dest[2]
wrist_angle = dest[3]
wrist_rotate = dest[4]
gripper = True
def arrCmp(arr1, arr2):
print('going')
def main():
arm = InterbotixRobot(robot_name="wx250", mrd=mrd)
arm.go_to_home_pose()
arm.set_ee_pose_components(x=0.2, y=0.1, z=0.2, roll=1.0, pitch=1.5)
arm.go_to_home_pose()
arm.go_to_sleep_pose()
