def __init__(self, speed=0.28, accuracy=0.003726646, timeout=5):
self.sawyer_arm = intera_interface.Limb('right')
self.joint_speed = speed
self.accuracy = accuracy
self.timeout = timeout
self.waypoints = list()
self._is_recording_waypoints = False
self.rs = intera_interface.RobotEnable()
self._init_state = self.rs.state().enabled
self.rs.enable()
# Create Navigator
self.navigator_io = intera_interface.Navigator()
self.sawyer_lights = intera_interface.Lights()
# Create Gripper & Cuff
self.sawyer_gripper = intera_interface.Gripper()
self.sawyer_cuff = intera_interface.Cuff(limb='right')
self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback,
'{0}_button_upper'.format('right'))
self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback,
'{0}_button_lower'.format('right'))
# print(self.navigator_io.list_all_items())
self.sawyer_arm.move_to_neutral(timeout=5, speed=self.joint_speed)
self.sawyer_gripper.open()
self.set_light_status('red', False)
self.set_light_status('green', False)
self.set_light_status('blue', True)
def __init__(self):
self.waypoints = Waypoints()
self.rs = intera_interface.RobotEnable()
self._init_state = self.rs.state().enabled
self.rs.enable()
# Set up arm, cuff and gripper
self.sawyer_arm = intera_interface.Limb('right')
self.sawyer_gripper = intera_interface.Gripper()
self.sawyer_cuff = intera_interface.Cuff(limb='right')
self.sawyer_lights = intera_interface.Lights()
# Move to default position when the ik solver is initially launched
self.sawyer_arm.move_to_neutral(timeout=10, speed=0.28)
self.sawyer_gripper.open()
self.gripper_dist = self.sawyer_gripper.get_position()
self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right'))
self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right'))
self.set_light_status('red', False)
self.set_light_status('green', True)
self.set_light_status('blue', False)
def __init__(self):
self.is_adding_new_item = False # This flag will be set to true if the user is in close proximity to the robot, flex_ik_service_client should immediatly exit
self.has_returned_home = False # False implies "The user wants to create an object, but im not in the default position, so I'll move there now"
node = rospy.init_node("sawyer_ik_solver_node")
rate = rospy.Rate(10) # Publishing rate in Hz
self.arm_speed = 0.3
self.arm_timeout = 5
self.hover_dist = 0.06
# Subscribe to topic where sortable messages arrive
ik_sub_sorting = rospy.Subscriber('ui/sortable_object/sorting/execute', SortableObjectMsg, callback=self.sort_object_callback, queue_size=20)
# Publish to this topic once object has been sorted, or if we got an error (data.successful_sort == False)
self.ik_pub_sorted = rospy.Publisher('sawyer_ik_sorting/sortable_objects/object/sorted', SortableObjectMsg, queue_size=10)
self.ik_sub_abort_sorting = rospy.Subscriber('ui/user/has_aborted', Bool, callback=None, queue_size=10) # This will suspend all sorting
self.ik_sub_add_item = rospy.Subscriber('ui/user/is_moving_arm', Bool, callback=self.disable_sorting_capability_callback, queue_size=10)
self.ik_pub_current_arm_pose = rospy.Publisher('sawyer_ik_sorting/sawyer_arm/pose/current', PoseGrippMessage, queue_size=10) # Publish current arm pose
self.ik_sub_locating_object_done = rospy.Subscriber('ui/new_object/state/is_located', Bool, callback=self.send_current_pose_callback, queue_size=10)
# Starts-up/enables the robot
try:
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
rs.enable()
except Exception as ex:
print(ex)
error_name = "IK solver crashed!"
error_msg = "The Inverse Kinematic solver has crashed. Moving the robot is no longer possible.\n\nPlese restart the program."
self.ik_solver_error_msg(error_name, error_msg) # Display error if robot can't be enabled
rospy.signal_shutdown("Failed to enable Robot")
ik_sub_shutdown = rospy.Subscriber('sawyer_ik_solver/change_to_state/shudown', Bool, callback=self.shutdown_callback, queue_size=10) # Topic where main_gui tells solver to shutdown
# Set up arm, cuff and gripper
self.sawyer_arm = intera_interface.Limb('right')
self.sawyer_gripper = intera_interface.Gripper()
self.sawyer_cuff = intera_interface.Cuff(limb='right')
self.sawyer_lights = intera_interface.Lights()
# Move to default position when the ik solver is initially launched
self.sawyer_arm.move_to_neutral(timeout=self.arm_timeout, speed=self.arm_speed)
self.sawyer_gripper.open()
self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right'))
self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right'))
self.set_light_status('red', False)
self.set_light_status('green', False)
self.set_light_status('blue', True)
rospy.spin()
def __init__(self, config=None):
"""Initialize.
See the parent class.
"""
super(FrankaPandaReal, self).__init__(config=config)
if rospy.get_name() == '/unnamed':
rospy.init_node('franka_panda')
assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().'
tic = time.time()
rospy.loginfo('Initializing robot...')
self._head = intera_interface.Head()
self._display = intera_interface.HeadDisplay()
self._lights = intera_interface.Lights()
self._limb = intera_interface.Limb()
self._joint_names = self._limb.joint_names()
self.cmd = []
self._command_msg = JointCommand()
self._command_msg.names = self._joint_names
self._commanders = dict(velocity=None, torque=None)
try:
self._gripper = intera_interface.Gripper()
self._has_gripper = True
except Exception:
self._has_gripper = False
self._robot_enable = intera_interface.RobotEnable(True)
self._params = intera_interface.RobotParams()
self._motion_planning = self.config.MOTION_PLANNING
if self._motion_planning == 'moveit':
rospy.loginfo('Initializing moveit toolkit...')
moveit_commander.roscpp_initialize(sys.argv)
self._scene = moveit_commander.PlanningSceneInterface()
self._group = moveit_commander.MoveGroupCommander('right_arm')
toc = time.time()
rospy.loginfo('Initialization finished after %.3f seconds.'
% (toc - tic))
def __init__(self, camera_name): #, mode, half_res):
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
print "Initializing ROS Node"
rospy.init_node('sawyer_cameras', anonymous=True)
# Lock for multithreading
self._lock = threading.RLock()
# for image pipe
self._imagestream = None
self._imagestream_endpoints = dict()
self._imagestream_endpoints_lock = threading.RLock()
# get access to camera controls from Intera SDK
self._camera = intera_interface.Cameras()
if not self._camera.verify_camera_exists(camera_name):
rospy.logerr("Invalid camera name, exiting the example.")
return
self._camera_name = camera_name
# automatically close camera at start
self._camera.stop_streaming(self._camera_name)
self._camera_open = False
self._ar_tracking = False
# set constant ImageHeader structure
self._image_header = RR.RobotRaconteurNode.s.NewStructure(
"SawyerCamera_interface.ImageHeader")
self._camera_intrinsics = None
# set SawyerImage struct
self._image = RR.RobotRaconteurNode.s.NewStructure(
"SawyerCamera_interface.SawyerImage")
# get access to light controls from Intera SDK
self._lights = intera_interface.Lights()
def __init__(self,
limb="right",
hover_distance=0.250,
tip_name="right_hand_camera",
camera_name="right_hand_camera"):
self._limb_name = limb # string
self._tip_name = tip_name # string
self._hover_distance = hover_distance # in meters
self._camera_name = camera_name
self._limb = intera_interface.Limb(limb)
self._gripper = intera_interface.Gripper()
self._rp = intera_interface.RobotParams()
self._light = intera_interface.Lights()
self._cameras = intera_interface.Cameras()
self._camera_orientation = Quaternion(x=0.0, y=1.0, z=0.0, w=0.0)
## Create Kitchen Items
self.kitchen_objects = {}
self.kitchen_objects['ketchup'] = KitchenObject(
'ketchup', 0, 0.450, -0.455, 0.245)
self.kitchen_objects['mayonaise'] = KitchenObject(
'mayonaise', 1, 0.450, -0.553, 0.245)
self.kitchen_objects['barbecue'] = KitchenObject(
'barbecue', 2, 0.362, -0.453, 0.245)
self.kitchen_objects['salad'] = KitchenObject('salad', 3, 0.362,
-0.553, 0.245)
self.check_positions = [
(0.450, -0.455),
(0.450, -0.553),
(0.362, -0.453),
(0.362, -0.553),
]
self.detected_object_data = {
'frame': None,
'obj_id': None,
'obj_pose': None
}
def __init__(self):
# Initialize Sawyer
rospy.init_node('Sawyer_comm_node', anonymous=True)
intera_interface.HeadDisplay().display_image('logo.png')
# Publishing topics
suppress_cuff_interaction = rospy.Publisher('/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1)
self.endpoint_topic = rospy.Publisher('/EndpointInfo', EndpointInfo, queue_size=10)
# Subscribing topics
rospy.Subscriber('/robot/limb/right/endpoint_state', intera_core_msgs.msg.EndpointState, self.forwardEndpointState)
# Lights
self.lights = intera_interface.Lights()
for light in self.lights.list_all_lights():
self.lights.set_light_state(light,False)
# Initialization complete. Spin.
rospy.loginfo('Ready.')
r = rospy.Rate(10)
while not rospy.is_shutdown():
suppress_cuff_interaction.publish()
r.sleep()
def __init__(self,
config='cfg/sawyer_ctrl_config.yaml',
controlType='EEImpedance',
use_safenet=False,
node_name='sawyer_interface'):
"""
Initialize Sawyer Robot for control
Args:
use_safenet: bool
whether to use the Safenet to constrain ee positions
and velocities
node_name: str
name of the node for ROS
Attributes:
ee_position
ee_orientation
"""
self.log_start_times = []
self.log_end_times = []
self.cmd_start_times = []
self.cmd_end_times = []
# Connect to redis client
# use default port 6379 at local host.
self.config = YamlConfig(config)
self.redisClient = RobotRedis(**self.config['redis'])
self.redisClient.flushall()
# Timing
self.startTime = time.time()
self.endTime = time.time()
self.action_set = False
self.model = Model()
self.ee_name = self.config['sawyer']['end_effector_name']
if config is not None:
if self.config['controller']['interpolator']['type'] == 'linear':
interp_kwargs = {
'max_dx': 0.005,
'ndim': 3,
'controller_freq': 500,
'policy_freq': 20,
'ramp_ratio': 0.2
}
self.interpolator_pos = LinearInterpolator(**interp_kwargs)
self.interpolator_ori = LinearOriInterpolator(**interp_kwargs)
rospy.loginfo(
"Linear interpolator created with params {}".format(
interp_kwargs))
else:
self.interpolator_pos = None
self.interpolator_ori = None
self.interpolator_goal_set = False
start = time.time()
rospy.loginfo('Initializing Sawyer robot interface...')
try:
self._head = iif.Head()
self._display = iif.HeadDisplay()
self._lights = iif.Lights()
self._has_head = True
except:
rospy.logerr('No head found, head functionality disabled')
self._has_head = False
self._limb = iif.Limb(limb="right", synchronous_pub=False)
self._joint_names = self._limb.joint_names()
self.cmd = []
try:
self._gripper = iif.Gripper()
self._has_gripper = True
except:
rospy.logerr('No gripper found, gripper control disabled')
self._has_gripper = False
self._robot_enable = iif.RobotEnable(True)
self._params = iif.RobotParams()
self.blocking = False
# set up internal pybullet simulation for jacobian and mass matrix calcs.
self._clid = pb.connect(pb.DIRECT)
pb.resetSimulation(physicsClientId=self._clid)
# TODO: make this not hard coded
sawyer_urdf_path = self.config['sawyer']['arm']['path']
self._pb_sawyer = pb.loadURDF(
fileName=sawyer_urdf_path,
basePosition=self.config['sawyer']['arm']['pose'],
baseOrientation=pb.getQuaternionFromEuler(
self.config['sawyer']['arm']['orn']),
globalScaling=1.0,
useFixedBase=self.config['sawyer']['arm']['is_static'],
flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT
| pb.URDF_USE_INERTIA_FROM_FILE,
physicsClientId=self._clid)
# For pybullet dof
self._motor_joint_positions = self.get_motor_joint_positions()
try:
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
self._ik_service = True
except:
rospy.logerr('IKService from Intera timed out')
self._ik_service = False
self._joint_names = self.config['sawyer']['limb_joint_names']
self.free_joint_dict = self.get_free_joint_dict()
self.joint_dict = self.get_joint_dict()
self._link_id_dict = self.get_link_dict()
rospy.loginfo('Sawyer initialization finished after {} seconds'.format(
time.time() - start))
# Set desired pose to initial
self.neutral_joint_position = self.config['sawyer'][
'neutral_joint_angles'] #[0, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161]
self.prev_cmd = np.asarray(self.neutral_joint_position)
self.current_cmd = self.prev_cmd
self.reset_to_neutral()
self.default_control_type = self.config['controller']['selected_type']
self.default_params = self.config['controller']['Real'][
self.default_control_type]
self.redisClient.set(CONTROLLER_CONTROL_TYPE_KEY,
self.default_control_type)
self.redisClient.set(CONTROLLER_CONTROL_PARAMS_KEY,
json.dumps(self.default_params))
self.pb_ee_pos_log = []
self.pb_ee_ori_log = []
self.pb_ee_v_log = []
self.pb_ee_w_log = []
# Set initial redis keys
self._linear_jacobian = None
self._angular_jacobian = None
self._jacobian = None
self._calc_mass_matrix()
self._calc_jacobian()
self.update_redis()
self.update_model()
self.controlType = self.get_control_type()
self.control_dict = self.get_controller_params()
self.controller = self.make_controller_from_redis(
self.controlType, self.control_dict)
self.redisClient.set(ROBOT_CMD_TSTAMP_KEY, time.time())
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, 0.1)
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time())
self.last_cmd_tstamp = self.get_cmd_tstamp()
self.last_gripper_cmd_tstamp = self.get_gripper_cmd_tstamp()
self.prev_gripper_state = self.des_gripper_state
rospy.logdebug('Control Interface initialized')
# TIMING TEST ONLY
self.timelog_start = open('cmd_tstamp.txt', 'w')
self.timelog_end = open('cmd_set_time.txt', 'w')
self.controller_times = []
self.loop_times = []
self.cmd_end_time = []
def __init__(self, enabled=True): self._light=intera_interface.Lights() self._enabled=enabled
def __init__(self):
# Initialize Sawyer
rospy.init_node('Sawyer_comm_node', anonymous=True)
intera_interface.HeadDisplay().display_image('logo.png')
# Publishing topics
suppress_cuff_interaction = rospy.Publisher(
'/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1)
self.position_topic = rospy.Publisher('/position',
JointInfo,
queue_size=1)
self.velocity_topic = rospy.Publisher('/velocity',
JointInfo,
queue_size=1)
self.effort_topic = rospy.Publisher('/effort', JointInfo, queue_size=1)
self.dev_topic = rospy.Publisher('/dev_topic', Int32, queue_size=10)
self.scroll_wheel_button_topic = rospy.Publisher(
'/scroll_wheel_button_topic', Empty, queue_size=10)
self.command_complete_topic = rospy.Publisher('/command_complete',
Empty,
queue_size=1)
self.wheel_delta_topic = rospy.Publisher('/wheel_delta',
Int32,
queue_size=10)
self.clicked = rospy.Publisher('scroll_wheel_pressed',
Bool,
queue_size=10)
self.toggle_completed_topic = rospy.Publisher('/toggle_completed',
Empty,
queue_size=1)
self.highTwo_success_topic = rospy.Publisher('/highTwo_success',
Bool,
queue_size=1)
self.endpoint_topic = rospy.Publisher('/EndpointInfo',
EndpointInfo,
queue_size=10)
self.interaction_control_topic = rospy.Publisher('/InteractionControl',
InteractionControl,
queue_size=10)
self.pickedup_topic = rospy.Publisher('/pickedup', Bool, queue_size=1)
self.pos_orient_topic = rospy.Publisher('/pos_orient',
String,
queue_size=1)
# Subscribing topics
rospy.Subscriber('/audio_duration', Float64, self.rx_audio_duration)
rospy.Subscriber('/robot/joint_states', sensor_msgs.msg.JointState,
self.forwardJointState)
rospy.Subscriber('/GoToJointAngles', GoToJointAngles,
self.cb_GoToJointAngles)
rospy.Subscriber('/wheel_subscription', Bool,
self.cb_WheelSubscription)
rospy.Subscriber('/joint_move', joint_move, self.cb_joint_move)
rospy.Subscriber('/InteractionControl', InteractionControl,
self.cb_interaction)
rospy.Subscriber('/JointAngle', String, self.cb_joint_angle)
rospy.Subscriber('/JointImpedance', JointImpedance, self.cb_impedance)
rospy.Subscriber('/multiple_choice', Bool, self.cb_multiple_choice)
rospy.Subscriber('/highTwo', Bool, self.cb_highTwo)
rospy.Subscriber('/robot/limb/right/endpoint_state',
intera_core_msgs.msg.EndpointState,
self.forwardEndpointState)
rospy.Subscriber('/adjustPoseTo', adjustPoseTo, self.cb_adjustPoseTo)
rospy.Subscriber('/closeGripper', Bool, self.cb_closeGripper)
rospy.Subscriber('/openGripper', Bool, self.cb_openGripper)
rospy.Subscriber('/GoToCartesianPose', GoToCartesianPose,
self.cb_GoToCartesianPose)
rospy.Subscriber('/cuffInteraction', cuffInteraction,
self.cb_cuffInteraction)
rospy.Subscriber('/check_pickup', Bool, self.cb_check_pickup)
rospy.Subscriber('/adjustPoseBy', adjustPoseBy, self.cb_adjustPoseBy)
rospy.Subscriber('/camera', Bool, self.cb_camera)
rospy.Subscriber('/robot/digital_io/right_lower_button/state',
intera_core_msgs.msg.DigitalIOState,
self.cb_cuff_lower)
rospy.Subscriber('/robot/digital_io/right_upper_button/state',
intera_core_msgs.msg.DigitalIOState,
self.cb_cuff_upper)
# Global Vars
self.audio_duration = 0
self.finished = False
self.startPos = 0
self.devMode = False
self.seqArr = []
self.p_command = lambda self: self.pub_num(180 / math.pi * self.limb.
joint_angle(shoulder))
wheel = self.finished
b_less = lambda self: self.limb.joint_angle(shoulder) < point_b[0]
b_more = lambda self: self.limb.joint_angle(shoulder) > point_b[0]
dot_2 = lambda self: self.limb.joint_angle(shoulder) < joint_dot_2[0]
#endpoint = lambda self : self.endpoints_equal(self.limb.endpoint_pose(),dot_3,tol) or self.finished
# Limb
self.limb = LimbPlus()
self.limb.go_to_joint_angles()
# Lights
self.lights = intera_interface.Lights()
for light in self.lights.list_all_lights():
self.lights.set_light_state(light, False)
# Navigator
self.navigator = intera_interface.Navigator()
self.multi_choice_callback_ids = self.BUTTON.copy()
for key in self.multi_choice_callback_ids:
self.multi_choice_callback_ids[key] = -1
self.wheel_callback_id = -1
self.wheel_state = self.navigator.get_wheel_state('right_wheel')
self.navigator.register_callback(self.rx_finished, 'right_button_ok')
self.navigator.register_callback(self.rx_cheat_code, 'head_button_ok')
# Gripper
self.gripper = intera_interface.get_current_gripper_interface()
if isinstance(self.gripper, intera_interface.SimpleClickSmartGripper):
if self.gripper.needs_init():
self.gripper.initialize()
else:
if not (self.gripper.is_calibrated()
or self.gripper.calibrate() == True):
raise
self.open_gripper()
# Cuff
self.cuff = intera_interface.Cuff()
self.cuff_callback_ids = self.CUFF_BUTTON.copy()
for key in self.cuff_callback_ids:
self.cuff_callback_ids[key] = -1
# Initialization complete. Spin.
rospy.loginfo('Ready.')
r = rospy.Rate(10)
while not rospy.is_shutdown():
suppress_cuff_interaction.publish()
r.sleep()
def main(arg):
rospy.init_node("Sawyer_DMP")
limb = intera_interface.Limb('right')
lights = intera_interface.Lights()
lights.set_light_state('right_hand_red_light')
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMWIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMHEIGHT)
if not cap.isOpened():
exit(1)
detection_graph, sess = detector_utils.load_inference_graph()
position_dmp = DMP(arg.gain, arg.num_gaussians, arg.stabilization,
arg.obstacle_avoidance, arg.obstacle_gain)
orientation_dmp = DMP(arg.gain, arg.num_gaussians, arg.stabilization,
False) # no obstacle avoidance on orientation
data = load_demo(arg.input_file)
t, q = parse_demo(data)
s = position_dmp.phase(t)
psv = position_dmp.distributions(s)
t = normalize_vector(t)
print('Calculating forward kinematics...')
robot = Sawyer()
Te = sp.lambdify(robot.q, robot.get_T_f()[-1])
ht = np.array([Te(*robot.JointOffset(qi)) for qi in q])
position, orientation = np.zeros((len(t), 3)), np.zeros((len(t), 3))
prev = None
for i, h in enumerate(ht):
position[i] = h[:3, 3]
prev = orientation[i] = transform_to_euler(h, prev)
print('Smoothing and filtering cartesian trajectory...')
dposition, ddposition = np.zeros((2, position.shape[0], position.shape[1]))
for i in range(position.shape[1]):
position[:, i] = smooth_trajectory(position[:, i], arg.window)
dposition[:, i] = vel(position[:, i], t)
ddposition[:, i] = vel(dposition[:, i], t)
dorientation, ddorientation = np.zeros(
(2, orientation.shape[0], orientation.shape[1]))
for i in range(position.shape[1]):
orientation[:, i] = smooth_trajectory(orientation[:, i], arg.window)
dorientation[:, i] = vel(orientation[:, i], t)
ddorientation[:, i] = vel(dorientation[:, i], t)
# Filter the position velocity and acceleration signals
f_position, f_dposition, f_ddposition = np.zeros(
(3, position.shape[0], position.shape[1]))
for i in range(position.shape[1]):
f_position[:, i] = blend_trajectory(position[:, i], dposition[:, i], t,
arg.blends)
f_dposition[:, i] = vel(f_position[:, i], t)
f_ddposition[:, i] = vel(f_dposition[:, i], t)
f_orientation, f_dorientation, f_ddorientation = np.zeros(
(3, orientation.shape[0], orientation.shape[1]))
for i in range(position.shape[1]):
f_orientation[:, i] = blend_trajectory(orientation[:, i],
dorientation[:,
i], t, arg.blends)
f_dorientation[:, i] = vel(f_orientation[:, i], t)
f_ddorientation[:, i] = vel(f_dorientation[:, i], t)
print('DMP: Imitating trajectory...')
ftarget_position, w_position = position_dmp.imitate(
position, dposition, ddposition, t, s, psv)
ftarget_orientation, w_orientation = orientation_dmp.imitate(
orientation, dorientation, ddorientation, t, s, psv)
lights.set_light_state('right_hand_green_light')
lights.set_light_state('right_hand_red_light', False)
obstacles, image_o = detect_hand(cap, detection_graph, sess, cap.get(3),
cap.get(4))
target, image_t = detect_block(cap)
if arg.show_camera:
for i in range(20):
obstacles, image_o = detect_hand(cap, detection_graph, sess,
cap.get(3), cap.get(4))
target, image_t = detect_block(cap)
cv2.imshow('hand', cv2.cvtColor(image_o, cv2.COLOR_RGB2BGR))
cv2.imshow('blocks', image_t)
cv2.imwrite(arg.output_file + '-obstacle-' + str(i) + '.png',
cv2.cvtColor(image_o, cv2.COLOR_RGB2BGR))
cv2.imwrite(arg.output_file + '-block-' + str(i) + '.png', image_t)
if cv2.waitKey(25) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
lights.set_light_state('right_hand_red_light')
lights.set_light_state('right_hand_green_light', False)
goal_pose = f_position[-1] if arg.original_point else [
target[0][0], target[0][1], f_position[-1][2]
]
obstacles = np.array([[0, 0, 0, 0, 0, 0]
]) if len(obstacles) is 0 else obstacles
print('DMP: Generating trajectory...')
ddxp, dxp, xp = position_dmp.generate(w_position, f_position[0], goal_pose,
t, s, psv, obstacles)
ddxo, dxo, xo = position_dmp.generate(w_orientation, f_orientation[0],
f_orientation[-1], t, s, psv,
obstacles)
print('Calculating inverse kinematics from DMP-cartesian trajectory...')
xc = np.zeros(q.shape)
prev_sol = q[0]
for i, xi in enumerate(xp):
if i > 0:
prev_sol = xc[i - 1]
ik = robot.Inverse_Kinematics(limb, xi, np.rad2deg(xo[i]), prev_sol)
xc[i] = [ik[j] for j in limb.joint_names()]
gen_sol = np.concatenate((xp, xo), axis=1)
real_sol = np.concatenate((position, orientation), axis=1)
w = np.hstack((w_position, w_orientation))
w_init = np.ones(w.shape)
ygaussianlabels = ['x', 'y', 'z', '$\\alpha$', '$\\beta$', '$\\gamma$']
# Mean squared error
mse = np.mean((real_sol - gen_sol)**2, axis=0)
print('Mean squared error:', mse)
print('start:', real_sol[0, :3])
print('end:', gen_sol[-1, :3])
print('goal:', goal_pose)
print('obstacles:', obstacles)
if arg.show_plots:
plot.comparison(t,
None,
q,
xc,
labels=[',', 'Original', 'Adapted'],
directory=arg.output_file)
plot.gaussian(s,
psv,
w_init,
"Initial Gaussian",
directory=arg.output_file,
ylabel=ygaussianlabels)
plot.gaussian(s,
psv,
w,
"Altered Gaussian",
directory=arg.output_file,
ylabel=ygaussianlabels)
plot.phase(s, directory=arg.output_file)
#plot.comparison(t, q, xc, None, labels=['Original q', 'cartesian-DMP', 'None'])
#plot.position(t, xo, position[:,3:], title='Orientation')
plot.cartesian_history([gen_sol, real_sol, obstacles],
[0.2, 0.2, 100.0],
directory=arg.output_file)
#plot.show_all()
print('Saving joint angle solutions to: %s' %
(arg.output_file + '_trajectory.txt'))
traj_final = np.concatenate(
(xc, np.multiply(np.ones((xc.shape[0], 1)), 0.0402075604203)), axis=1)
time = np.linspace(0, t[-1], xc.shape[0]).reshape((xc.shape[0], 1))
traj_final = np.concatenate((t.reshape((-1, 1)), traj_final), axis=1)
header = 'time,right_j0,right_j1,right_j2,right_j3,right_j4,right_j5,right_j6,right_gripper'
np.savetxt(arg.output_file + '_trajectory.txt',
traj_final,
delimiter=',',
header=header,
comments='',
fmt="%1.12f")
lights.set_light_state('right_hand_red_light', False)
#!/usr/bin/env python2
import numpy as np
import rospy
import intera_interface
rospy.init_node("intera-joint-recorder")
# Create an object to interface with the arm
limb = intera_interface.Limb('right')
gripper = intera_interface.Gripper('right')
cuff = intera_interface.Cuff('right')
lights = intera_interface.Lights()
# Move the robot to the starting point
angles = limb.joint_angles()
angles['right_j0'] = np.radians(0)
angles['right_j1'] = np.radians(-50)
angles['right_j2'] = np.radians(0)
angles['right_j3'] = np.radians(120)
angles['right_j4'] = np.radians(0)
angles['right_j5'] = np.radians(0)
angles['right_j6'] = np.radians(0)
limb.move_to_joint_positions(angles)
q = []
t = []
lights.set_light_state('right_hand_red_light')
while not cuff.lower_button():