duration=1.0,
# soft_position_control=True
)
position_control_mode = blue._control_mode # super sketchy
# Set up a publisher for our cartesian pose command (a little sketchy)
RBC = blue._RBC
pose_target_publisher = RBC.publisher(
"/{}_arm/pose_target/command".format(arm_side),
"geometry_msgs/PoseStamped")
# 3D mouse ~~~
mouse = spacemouse.SpaceMouse()
# Initial position
target_pose = blue.get_cartesian_pose()
target_position = target_pose["position"]
target_orientation = target_pose["orientation"]
# Gripper state
gripper_closed = False
#
prev_input_button0 = False
disabled = False
print("Ready!")
while True:
if not mouse.input_button0 and prev_input_button0:
#!/usr/bin/env python3
# A basic example of reading information about Blue's current state.
import time
import numpy as np
from blue_interface import BlueInterface
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
def print_aligned(left, right):
print("{:30} {}".format(left, np.round(right, 4)))
while True:
print_aligned("End Effector Position:", blue.get_cartesian_pose()["position"])
print_aligned("End Effector Orientation:", blue.get_cartesian_pose()["orientation"])
print_aligned("Joint Positions:", blue.get_joint_positions())
print_aligned("Joint Velocities:", blue.get_joint_velocities())
print_aligned("Joint Torques:", blue.get_joint_torques())
print_aligned("Gripper Position:", blue.get_gripper_position())
print_aligned("Gripper Effort:", blue.get_gripper_effort())
print("=" * 30)
time.sleep(0.5)
) #this turns off any other control currently on the robot (leaves it in gravtiy comp mode)
joint_angle_list = [] #Initialize the list to hold our joint positions
pose_list = []
gripper_list = []
input(
"Press enter to start recording. To finish recording press <ctrl+c>.")
try:
last_time = 0.0
while True:
position = blue.get_joint_positions(
) #record the pose, this function returns a dictionary object
joint_angle_list.append(position)
pose = blue.get_cartesian_pose()
pose_list.append(pose)
gripper_pos = blue.get_gripper_position()
gripper_list.append(gripper_pos)
print("Position recorded!")
#while time.time() - last_time < 1.0/frequency:
# pass
sleep_time = 1.0 / frequency - (time.time() - last_time)
if sleep_time > 0:
time.sleep(sleep_time)
last_time = time.time()
except:
print(joint_angle_list)
if len(joint_angle_list) == 0:
print('You did not save any positions')
#!/usr/bin/env python3
# A basic example of reading information about Blue's current state.
from blue_interface import BlueInterface
import numpy as np
import time
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
print_aligned = lambda left, right: print("{:30} {}".format(
left, np.round(right, 4)))
while True:
print_aligned("End Effector Position:",
blue.get_cartesian_pose()['position'])
print_aligned("End Effector Orientation:",
blue.get_cartesian_pose()['orientation'])
print_aligned("Joint Positions:", blue.get_joint_positions())
print_aligned("Joint Velocities:", blue.get_joint_velocities())
print_aligned("Joint Torques:", blue.get_joint_torques())
print_aligned("Gripper Position:", blue.get_gripper_position())
print_aligned("Gripper Effort:", blue.get_gripper_effort())
print("=" * 30)
time.sleep(0.5)
