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():
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 move_to_position(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=position[0], z=position[2], blocking=True)
arm.set_ee_cartesian_trajectory(x=-0.05)
arm.set_single_joint_position("waist", 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="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()
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()
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()
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()
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()