class Baxter_Interactive(object):
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def recorder(self):
doneRecording = False
while not doneRecording:
if self._navigator.button0:
self.recorded.append(self._limb.joint_angles())
print 'Waypoint Added'
rospy.sleep(1)
if self._close_button.state:
self.recorded.append('CLOSE')
self._gripper.close()
print 'Gripper Closed'
rospy.sleep(1)
if self._open_button.state:
self.recorded.append('OPEN')
self._gripper.open()
print 'Gripper Opened'
rospy.sleep(1)
if self._navigator.button1:
print 'END RECORDING'
doneRecording = True
rospy.sleep(3)
while doneRecording:
for waypoint in self.recorded:
if waypoint == 'OPEN':
self._gripper.open()
rospy.sleep(1)
elif waypoint == 'CLOSE':
self._gripper.close()
rospy.sleep(1)
else:
self._limb.move_to_joint_positions(waypoint)
class Baxter(object):
"""
Iinterface for controlling the real and simulated
Baxter robot.
"""
def __init__(self,
sim=False,
config=None,
time_step=1.0,
rate=100.0,
missed_cmds=20000.0):
"""
Initialize Baxter Interface
Args:
sim (bool): True specifies using the PyBullet simulator
False specifies using the real robot
arm (str): 'right' or 'left'
control (int): Specifies control type
TODO: change to str ['ee', 'position', 'velocity']
config (class): Specifies joint ranges, ik null space paramaters, etc
time_step (float):
TODO: probably get rid of this
rate (float):
missed_cmds (float):
"""
self.sim = sim
self.dof = {
'left': {
'ee': 6,
'position': 7,
'velocity': 7
},
'right': {
'ee': 6,
'position': 7,
'velocity': 7
},
'both': {
'ee': 12,
'position': 14,
'velocity': 14
}
}
if self.sim:
import pybullet as p
import baxter_pybullet_interface as pybullet_interface
self.baxter_path = "assets/baxter_robot/baxter_description/urdf/baxter.urdf"
self.time_step = time_step
else:
import rospy
from baxter_pykdl import baxter_kinematics
import baxter_interface
from baxter_interface import CHECK_VERSION, Limb, Gripper
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from std_msgs.msg import Header
from baxter_core_msgs.srv import SolvePositionIK, SolvePositionIKRequest
self.rate = rate
self.freq = 1 / rate
self.missed_cmds = missed_cmds
self.config = config
self.initial_setup()
def set_command_time_out(self):
"""
Set command timeout for sending ROS commands
"""
if self.sim:
pass
else:
self.left_arm.set_command_timeout(self.freq * self.missed_cmds)
self.right_arm.set_command_timeout(self.freq * self.missed_cmds)
return
def initial_setup(self):
if self.sim:
# load baxter
objects = [p.loadURDF(self.baxter_path, useFixedBase=True)]
self.baxter_id = objects[0]
print("baxter_id: ", self.baxter_id)
if self.config:
self.use_nullspace = True
self.use_orientation = True
self.ll = self.config.nullspace.ll
self.ul = self.config.nullspace.ul
self.jr = self.config.nullspace.jr
self.rp = self.config.nullspace.rp
else:
self.use_nullspace = False
self.create_arms()
else:
self.create_arms()
self.set_command_time_out()
self.control_rate = rospy.Rate(self.rate)
def reset(self, initial_pose=None):
if self.sim:
self._reset_sim(initial_pose)
else:
self._reset_real(initial_pose)
def _reset_real(self, initial_pose=None):
self.enable()
time.sleep(0.5)
self.set_initial_position('right')
self.set_initial_position('left', blocking=True)
self.calibrate_grippers()
def _reset_sim(self, control_type=None, initial_pose=None):
# set control type
if control_type == 'position' or control_type == 'ee' or control_type is None:
control_mode = p.POSITION_CONTROL
elif control_type == 'velocity':
control_mode = p.VELOCITY_CONTROL
elif control_mode == 'torque':
control_mode = p.TORQUE_CONTROL
else:
raise ValueError(
"control_type must be in ['position', 'ee', 'velocity', 'torque']."
)
# set initial position
if initial_pose:
for joint_index, joint_val in initial_pose:
p.resetJointState(0, joint_index, joint_val)
p.setJointMotorControl2(self.baxter_id,
jointIndex=joint_index,
controlMode=control_mode,
targetPosition=joint_val)
else:
num_joints = p.getNumJoints(self.baxter_id)
for joint_index in range(num_joints):
p.setJointMotorControl2(self.baxter_id,
joint_index,
control_mode,
maxForce=self.config.max_force)
return
def create_arms(self):
"""
Create arm interface objects for Baxter.
An arm consists of a Limb and its Gripper.
"""
# create arm objects
if self.sim:
self.left_arm = pybullet_interface.Limb(self.baxter_id, "left",
self.config)
# self.left_arm.gripper = pybullet_interface.Gripper("left")
self.right_arm = pybullet_interface.Limb(self.baxter_id, "right",
self.config)
# self.right_arm.gripper = pybullet_interface.Gripper("right")
else:
self.left_arm = Limb("left")
self.left_arm.gripper = Gripper("left")
self.left_arm.kin = baxter_kinematics("left")
self.right_arm = Limb("right")
self.right_arm.gripper = Gripper("right")
self.right_arm.kin = baxter_kinematics("right")
return
def calibrate_grippers(self):
"""
(Blocking) Calibrates gripper(s) if not
yet calibrated
"""
if not self.left_arm.gripper.calibrated():
self.left_arm.gripper.calibrate()
if not self.right_arm.gripper.calibrated():
self.right_arm.gripper.calibrate()
return
def enable(self):
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
def shutdown(self):
pass
def set_joint_velocities(self, arm, joint_velocities):
"""
Set joint velocites for a given arm
Args
arm (str): 'left' or 'right' or 'both'
joint_velocities (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'velocity',
joint_velocities)
self.left_arm.set_joint_velocities(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'velocity',
joint_velocities)
self.right_arm.set_joint_velocities(action_dict)
elif arm == 'both':
l_velocities = joint_velocities[:7]
r_velocities = joint_velocities[7:]
l_action_dict = self.create_action_dict('left', 'velocity',
l_velocities)
r_action_dict = self.create_action_dict('right', 'velocity',
r_velocities)
self.left_arm.set_joint_velocities(l_action_dict)
self.right_arm.set_joint_velocities(r_action_dict)
return
def move_to_joint_positions(self, arm, joint_positions):
"""
Move specified arm to give joint positions
(This function is blocking.)
Args
arm (str): 'left' or 'right' or 'both'
joint_positionns (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'position',
joint_positions)
self.left_arm.move_to_joint_positions(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'position',
joint_positions)
self.right_arm.move_to_joint_positions(action_dict)
elif arm == 'both':
l_joints = joint_positions[:7]
r_joints = joint_positions[7:]
l_action_dict = self.create_action_dict('left', 'position',
l_joints)
r_action_dict = self.create_action_dict('right', 'position',
r_joints)
self.left_arm.move_to_joint_positions(l_action_dict)
self.right_arm.move_to_joint_positions(r_action_dict)
return
def set_joint_positions(self, arm, joint_positions):
"""
Move specified arm to give joint positions
(This function is not blocking.)
Args
arm (str): 'left' or 'right' or 'both'
joint_positionns (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'position',
joint_positions)
self.left_arm.set_joint_positions(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'position',
joint_positions)
self.right_arm.set_joint_positions(action_dict)
elif arm == 'both':
l_joints = joint_positions[:7]
r_joints = joint_positions[7:]
l_action_dict = self.create_action_dict('left', 'position',
l_joints)
r_action_dict = self.create_action_dict('right', 'position',
r_joints)
self.left_arm.set_joint_positions(l_action_dict)
self.right_arm.set_joint_positions(r_action_dict)
return
def move_to_ee_pose(self, arm, pose, blocking=True):
"""
Move end effector to specified pose
Args
arm (string): "left" or "right" or "both"
pose (list):
if arm == 'left' or arm == 'right':
pose = [X, Y, Z, r, p, w]
else:
pose = left_pose + right _pose
"""
if arm == "both":
left_pose = pose[:6]
right_pose = pose[6:]
left_joints = self.ik('left', left_pose)
right_joints = self.ik('right', right_pose)
if blocking:
self.left_arm.move_to_joint_positions(left_joints)
self.right_arm.move_to_joint_positions(right_joints)
else:
self.left_arm.set_joint_positions(left_joints)
self.right_arm.set_joint_positions(right_joints)
elif arm == "left":
joints = self.ik(arm, pose)
if blocking:
self.left_arm.move_to_joint_positions(joints)
else:
self.left_arm.set_joint_positions(joints)
elif arm == "right":
joints = self.ik(arm, pose)
if blocking:
self.right_arm.move_to_joint_positions(joints)
else:
self.right_arm.set_joint_positions(joints)
else:
raise ValueError("Arm must be 'right', 'left', or 'both'")
return
def get_ee_pose(self, arm, mode=None):
"""
Returns end effector pose for specified arm.
End effector pose is a list of values corresponding to the 3D cartesion coordinates
and roll (r), pitch (p), and yaw (w) Euler angles.
Args
arm (string): "right" or "left"
mode (string): "Quaternion" or "quaterion" or "quat"
Returns
pose (list): Euler angles [X,Y,Z,r,p,w] or Quaternion [X,Y,Z,x,y,z,w]
"""
if arm == 'left' or arm == 'right':
pos = self.get_ee_position(arm)
orn = self.get_ee_orientation(arm, mode)
return pos + orn
elif arm == 'both':
l_pos = self.get_ee_position('left')
l_orn = self.get_ee_orientation('left', mode)
r_pos = self.get_ee_position('right')
r_orn = self.get_ee_orientation('right', mode)
return l_pos + l_orn + r_pos + r_orn
def get_ee_position(self, arm):
"""
Returns end effector position for specified arm.
Returns the 3D cartesion coordinates of the end effector.
Args
arm (string): "left" or "right" or "both"
Returns
[X,Y,Z]
"""
if arm not in ['left', 'right', 'both']:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if arm == "left":
return list(self.left_arm.endpoint_pose()['position'])
elif arm == "right":
return list(self.right_arm.endpoint_pose()['position'])
elif arm == "both":
return list(self.left_arm.endpoint_pose()['position']) + list(
self.right_arm.endpoint_pose()['position'])
def get_ee_orientation(self, arm, mode=None):
"""
Returns a list of the orientations of the end effectors(s)
Args
arm (string): "right" or "left" or "both"
mode (string): specifies angle representation
Returns
orn (list): list of Euler angles or Quaternion
"""
if arm not in ['left', 'right', 'both']:
raise ValueError("Arg arm should be 'left' or 'right'.")
if arm == "left":
orn = list(self.left_arm.endpoint_pose()['orientation'])
elif arm == "right":
orn = list(self.right_arm.endpoint_pose()['orientation'])
elif arm == "both":
orn = list(self.left_arm.endpoint_pose()['orientation']) + list(
self.right_arm.endpoint_pose()['orientation'])
return list(p.getEulerFromQuaternion(orn))
def get_joint_angles(self, arm):
"""
Get joint angles for specified arm
Args
arm(strin): "right" or "left" or "both"
Returns
joint_angles (list): List of joint angles starting from the right_s0 ('right_upper_shoulder')
and going down the kinematic tree to the end effector.
"""
if arm == "left":
joint_angles = self.left_arm.joint_angles()
elif arm == "right":
joint_angles = self.right_arm.joint_angles()
elif arm == "both":
joint_angles = self.left_arm.joint_angles(
) + self.right_arm.joint_angles()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_angles = joint_angles.values()
return joint_angles
def get_joint_velocities(self, arm):
"""
Get joint velocites for specified arm
"""
if arm == "left":
return self.left_arm.joint_velocities()
elif arm == "right":
return self.right_arm.joint_velocities()
elif arm == "both":
return self.left_arm.joint_velocities(
) + self.right_arm.joint_velocities()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_velocities = joint_velocities.values()
return joint_velocities
def get_joint_efforts(self, arm):
"""
Get joint torques for specified arm
"""
if arm == "left":
return self.left_arm.joint_effort()
elif arm == "right":
return self.right_arm.joint_efforts()
elif arm == "both":
return self.left_arm.joint_efforts(
) + self.right_arm.joint_efforts()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_efforts = joint_efforts.values()
return
def apply_action(self, arm, control_type, action):
"""
Apply a joint action
Blocking on real robot
Args
action - list or tuple specifying end effector pose(6DOF * num_arms)
or joint angles (7DOF * num_arms)
"""
# verify action
verified, err = self._verify_action(arm, control_type, action)
if not verified:
raise err
# execute action
if control_type == 'position':
self._apply_position_control(arm, action)
elif control_type == 'ee':
self._apply_ee_control(arm, action)
elif control_type == 'velocity':
self._apply_velocity_control(arm, action)
elif control_type == 'torque':
self._apply_torque_control(arm, action)
else:
raise ValueError(
"Control type must be ['ee', 'position', 'velocity', 'torque']"
)
def _verify_action(self, arm, control_type, action):
"""
Verify type and dimension of action
Args
arm (str): "left" or "right" or "both"
action (list): list of floats len will vary depending on action type
Returns
bool: True if action is right dimension, false otherwise
"""
if arm not in ["left", "right", "both"]:
return False, ValueError("Arg arm must be string")
if control_type not in ['ee', 'position', 'velocity', 'torque']:
return False, ValueError(
"Arg control_type must be one of ['ee', 'position', 'velocity', 'torque']"
)
if not isinstance(action, (list, tuple, np.ndarray)):
return False, TypeError("Action must be a list or tuple.")
if len(action) != self.dof[arm][control_type]:
return False, ValueError("Action must have len {}".format(
self.dof * num_arms))
return True, ""
def _apply_torque_control(self, arm, action):
"""
As of right now, setting joint torques does not
does not command the robot as expected
"""
raise NotImplementedError(
'Cannot apply torque control. Try using joint position or velocity control'
)
def _apply_velocity_control(self, arm, action):
"""
Apply velocity control to a given arm
Args
arm (str): 'right' or 'left'
action (list, tuple, or numpy array) of len 7
"""
if self.sim:
pass
else:
if arm == 'left':
action_dict = self
self.right_arm.set_joint_velocities
if arm == 'right':
pass
if arm == 'both':
pass
return
def _apply_position_control(self, arm, action, blocking=True):
"""
Apply a joint action
Args:
arm (str): 'right', 'left', or 'both'
action - list or array of joint angles
blocking (bool): If true, wait for arm(s) to reach final position(s)
"""
action = list(action)
if blocking:
self.move_to_joint_positions(arm, action)
else:
self.set_joint_positions(arm, action)
return
def _apply_ee_control(self, arm, action, blocking=True):
"""
Apply action to move Baxter end effector(s)
Args
arm (str): 'left' or 'right' or 'both'
action (list, tuple, or numpy array)
blocking: Bool
"""
action = list(action)
if self.sim:
if arm == 'left' or arm == 'right':
self.move_to_ee_pose(arm, action)
elif arm == 'both':
if self.sim:
joint_indices = self.left_arm.indices + self.right_arm.indices
self.left_arm.move_to_joint_positions(
actions, joint_indices)
else:
self.move_to_ee_pose(arm, action, blocking)
return
def fk(self, arm, joints):
"""
Calculate forward kinematics
Args
arm (str): 'right' or 'left'
joints (list): list of joint angles
Returns
pose(list): [x,y,z,r,p,w]
"""
if arm == 'right':
pose = list(self.right_arm.kin.forward_position_kinematics(joints))
elif arm == 'left':
pose = list(self.left_arm.kin.forward_position_kinematics(joints))
else:
raise ValueError("Arg arm must be 'right' or 'left'")
return pose[:3] + list(self.quat_to_euler(pose[3:]))
def _ik(self, arm, pos, orn=None, seed=None):
"""
Calculate inverse kinematics
Args
arm (str): 'right' or 'left'
pos (list): [X, Y, Z]
orn (list): [r, p, w]
seed (int): for setting random seed
Returns
joint angles (list): A list of joint angles
Note: This will probably fail if orn is not included
"""
if orn is not None:
orn = list(self.euler_to_quat(orn))
if arm == 'right':
joint_angles = self.right_arm.kin.inverse_kinematics(pos, orn)
elif arm == 'left':
joint_angles = self.left_arm.kin.inverse_kinematics(pos, orn, seed)
else:
raise ValueError("Arg arm must be 'right' or 'left'")
return list(joint_angles.tolist())
def ik(self, arm, ee_pose):
"""
Calculate inverse kinematics for a given end effector pose
Args
ee_pose (list or tuple of floats): 6 dimensional array specifying
the position and orientation of the end effector
arm (string): "right" or "left"
Return
joints (list): A list of joint angles
"""
if self.sim:
joints = self._sim_ik(arm, ee_pose)
else:
joints = self._real_ik(arm, ee_pose)
if not joints:
print("IK failed. Try running again or changing the pose.")
return joints
def _sim_ik(self, arm, ee_pose):
"""
(Sim) Calculate inverse kinematics for a given end effector pose
Args:
ee_pose (tuple or list): [pos, orn] of desired end effector pose
pos - x,y,z
orn - r,p,w
arm (string): "right" or "left"
Returns:
joint_angles (list): List of joint angles
"""
if arm == 'right':
ee_index = self.right_arm.ee_index
elif arm == 'left':
ee_index = self.left_arm.ee_index
elif arm == 'both':
pass
else:
raise ValueError("Arg arm must be 'left', 'right', or 'both'.")
pos = ee_pose[:3]
orn = ee_pose[3:]
if (self.use_nullspace == 1):
if (self.use_orientation == 1):
joints = p.calculateInverseKinematics(self.baxter_id, ee_index,
pos, orn, self.ll,
self.ul, self.jr,
self.rp)
else:
joints = p.calculateInverseKinematics(self.baxter_id,
ee_index,
pos,
lowerLimits=self.ll,
upperLimits=self.ul,
jointRanges=self.jr,
restPoses=self.rp)
else:
if (self.use_orientation == 1):
joints = p.calculateInverseKinematics(self.baxter_id,
ee_index,
pos,
orn,
jointDamping=self.jd)
else:
joints = p.calculateInverseKinematics(self.baxter_id, ee_index,
pos)
return joints
def _real_ik(self, arm, ee_pose):
"""
(Real) Calculate inverse kinematics for a given end effector pose
Args:
ee_pose (tuple or list): [pos, orn] of desired end effector pose
pos - x,y,z
orn - r,p,w
Returns:
joint_angles (dict): Dictionary containing {'joint_name': joint_angle}
"""
pos = ee_pose[:3]
orn = ee_pose[3:]
orn = self.euler_to_quat(orn)
ns = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ik_pose = PoseStamped()
ik_pose.pose.position.x = pos[0]
ik_pose.pose.position.y = pos[1]
ik_pose.pose.position.z = pos[2]
ik_pose.pose.orientation.x = orn[0]
ik_pose.pose.orientation.y = orn[1]
ik_pose.pose.orientation.z = orn[2]
ik_pose.pose.orientation.w = orn[3]
ik_pose.header = hdr
ikreq.pose_stamp.append(ik_pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
except (rospy.ServiceException, rospy.ROSException), e:
# rospy.logerr("Service call failed: %s" % (e,))
return
class ArmController(object):
def __init__(self,
starting_poss=None,
push_thresh=10,
mode='one_arm',
arm_mode='first'):
"""
Initialises parameters and moves the arm to a neutral
position.
"""
self.push_thresh = push_thresh
self._right_neutral_pos = starting_poss[0]
self._left_neutral_pos = starting_poss[1]
self._mode = mode
self._arm_mode = arm_mode
rospy.loginfo("Creating interface and calibrating gripper")
self._right_limb = Limb('right')
self._left_limb = Limb('left')
self._right_gripper = Gripper('right', CHECK_VERSION)
self._right_gripper.calibrate()
self._left_gripper = Gripper('left', CHECK_VERSION)
self._left_gripper.calibrate()
self._is_right_fist_closed = False
self._is_left_fist_closed = False
rospy.loginfo("Moving to neutral position")
self.move_to_neutral()
rospy.loginfo("Initialising PoseGenerator")
self._pg = PoseGenerator(self._mode, self._arm_mode)
self._sub_right_gesture = rospy.Subscriber(
"/low_myo/gesture", UInt8, self._right_gesture_callback)
self._sub_left_gesture = rospy.Subscriber("/top_myo/gesture", UInt8,
self._left_gesture_callback)
self._last_data = None
self._pg.calibrate()
def move_to_neutral(self):
if self._mode == "one_arm":
self._right_limb.move_to_joint_positions(self._right_neutral_pos)
elif self._mode == "two_arms":
self._right_limb.move_to_joint_positions(self._right_neutral_pos)
self._left_limb.move_to_joint_positions(self._left_neutral_pos)
else:
raise ValueError("Mode %s is invalid!" % self._mode)
def is_right_pushing(self):
"""
Checks if any of the joints is under external stress. Returns
true if the maximum recorded stress above specified threshold.
"""
e = self._right_limb.joint_efforts()
max_effort = max([abs(e[i]) for i in e.keys()])
return max_effort > self.push_thresh
def is_left_pushing(self):
"""
Checks if any of the joints is under external stress. Returns
true if the maximum recorded stress above specified threshold.
"""
e = self._left_limb.joint_efforts()
max_effort = max([abs(e[i]) for i in e.keys()])
return max_effort > self.push_thresh
def _command_right_gripper(self):
"""
Reads state from Myo and opens/closes gripper as needed.
"""
if not self._right_gripper.ready():
return
if self._right_gripper.moving():
return
if self._is_right_fist_closed:
self._right_gripper.close()
else:
self._right_gripper.open()
def _command_left_gripper(self):
"""
Reads state from Myo and opens/closes gripper as needed.
"""
if not self._left_gripper.ready():
return
if self._left_gripper.moving():
return
if self._is_left_fist_closed:
self._left_gripper.close()
else:
self._left_gripper.open()
def step(self):
"""
Executes a step of the main routine.
Fist checks the status of the gripper and
"""
os.system('clear')
if self._mode == "one_arm":
return self.one_arm_step()
elif self._mode == "two_arms":
return self.two_arms_step()
else:
raise ValueError("Mode %s is invalid!" % self.mode)
def one_arm_step(self):
self._command_right_gripper()
pos = self._pg.generate_pose()
if pos is not None:
if not self.is_right_pushing():
self._right_limb.set_joint_positions(pos)
else:
rospy.logwarn("Arm is being pushed!")
else:
rospy.logwarn("Generated position is invalid")
def two_arms_step(self):
self._command_right_gripper()
self._command_left_gripper()
poss = self._pg.generate_pose()
if poss is not None:
if not self.is_right_pushing():
self._right_limb.set_joint_positions(poss[0])
if not self.is_left_pushing():
self._left_limb.set_joint_positions(poss[1])
else:
rospy.logwarn("Arm is being pushed!")
else:
rospy.logwarn("Generated position is invalid")
def _right_gesture_callback(self, data):
self._is_right_fist_closed = (data.data == 1)
def _left_gesture_callback(self, data):
self._is_left_fist_closed = (data.data != 0)
class get_pos(object):
def __init__(self, limb_viewer, limb_checker, parameter_reset):
'''
Viewer - Limb that will end up reaching out to the user's hand
Checker - Limb that checks whether the user's hand is still in position
Parameter Reset - Determines whether the camera parameters should be reset. Reset is only needed once
'''
#Initializing for the translation
self.left_arm = Limb('left')
self.right_arm = Limb('right')
self.limb = limb_viewer
self.img_viewer = None
self.arm_viewer = baxter_interface.Limb(
limb_viewer) # Assinging the viewer to the correct limb
self.arm_checker = baxter_interface.Limb(
limb_checker) # Assigning the checker to the correct limb
self.hand_area = 0 # Hand area is used by Checker to determine whether the hand is still detected
self.bridge = CvBridge() # ROS to Opencv
# Coordinates and global variables setup
self.torque = None # Torque will be used as a detection method
self.frame = None # Will be used for image shape
self.x = 0 # Will be assigned to x-pixel-coordinate of detected hand // Will later be converted to base frame
self.y = 0 # Will be assigned to y-pixel-coordinate of detected hand // Will later be converted to base frame
self.z_camera = 0 # Will be assigned to z spatial coordinate WRT to camera
# Arm sensor setup
self.distance = {}
root_name = "/robot/range/"
sensor_name = ["left_hand_range/state", "right_hand_range/state"]
# Assigning the camera topics to viewer and checker depending on the user input
if limb_viewer == 'left':
self.camera_viewer = 'left_hand_camera'
camera_checker = 'right_hand_camera'
# Subscribing to the left sensor
self.__sensor = rospy.Subscriber(root_name + sensor_name[0],
Range,
callback=self.sensorCallback,
callback_args="left",
queue_size=1)
else:
self.camera_viewer = 'right_hand_camera'
camera_checker = 'left_hand_camera'
# Subscribing to the right sensor
self.__sensor = rospy.Subscriber(root_name + sensor_name[1],
Range,
callback=self.sensorCallback,
callback_args="right",
queue_size=1)
# resetting the parameters of the viewer and checker if instructed
if parameter_reset == True:
settings = CameraController.createCameraSettings(width=640,
height=400,
exposure=-1)
CameraController.openCameras(self.camera_viewer,
camera_checker,
settings=settings,
settings2=settings)
print "Viewer-camera, checker-camera parameter check"
# self.left_camera = baxter_interface.CameraController(self.camera_viewer)
# self.left_camera.resolution = (640,400)
# self.left_camera.exposure = -1
# print "Viewer-camera parameter check"
# self.right_camera = baxter_interface.CameraController(camera_checker)
# self.right_camera.resolution = (640,400)
# self.right_camera.exposure = -1
# print "Checker-camera parameter check"
# Subscribing to the cameras
self.viewer_image_sub = rospy.Subscriber(
"/cameras/" + self.camera_viewer + "/image", Image,
self.callback_viewer) # Subscribing to Camera
self.checker_image_sub = rospy.Subscriber(
"/cameras/" + camera_checker + "/image", Image,
self.callback_checker) # Subscribing to Camera
# Rotation matrix set up
self.tf = TransformListener()
# Orientation of the shaker will determine how the viewer's orientation will be once it reaches its final position
self.left_orientation_shaker = [
-0.520, 0.506, -0.453, 0.518
] # Defined orientation // Obtained through tf_echo // ****GRIPPER****
self.right_orientation_shaker = [0.510, 0.550, 0.457, 0.478]
self.camera_gripper_offset = [
0.038, 0.012, -0.142
] # Offset of camera from the GRIPPER reference frame
self._cur_joint = {
'left_w0': 0.196733035822,
'left_w1': 0.699878733673,
'left_w2': 0,
'left_e0': -0.303344700458,
'left_e1': 1.90098568922,
'left_s0': -0.263844695215,
'left_s1': -1.03467004025
}
def follow_up(self, joint_solution=None):
'''
Any follow up instructions, after the hand is reached, should be in here
'''
if joint_solution == None:
joint_solution = self._cur_joint
self.__sensor.unregister() #
self.viewer_image_sub.unregister()
self.checker_image_sub.unregister()
self.arm_viewer.move_to_joint_positions(joint_solution)
def sensorCallback(self, msg, side):
self.distance[side] = msg.range
if msg.range < 0.05: # Only assigns a value to sensor if the hand is less than 10cm away
self.z_camera = msg.range # Assign the z-coordinate
else:
self.z_camera = None
def callback_viewer(self, data):
'''
This is the callback function of the viewer, i.e, the thread that the viewer creates once it's initialized.
The viewer callback first runs a skin color detection, creates a mask from the given color range, and then
the hand detection is ran on the mask. The hand detection is done through a cascade classifier, and the
coordinates of the hands are assigned to the global variables x and y. To be noted that the contour drawing
is only to provide a good display; it doesn't affect the skin detection, though it uses the same mask
'''
try:
# Creates an OpenCV image from the ROS image
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# The torque is used as a method of checking whether the arm has reached the user
self.torque = self.arm_viewer.endpoint_effort()
self.torque = self.torque_mag(
self.torque
) # The torque assigned is the magnitude of the torque detected
img = cv_image
converted = cv2.cvtColor(
img, cv2.COLOR_BGR2YCR_CB) # Convert image color scheme to YCrCb
min_YCrCb = np.array(
[0, 133, 77], np.uint8) # Create a lower bound for the skin color
max_YCrCb = np.array([255, 173, 127],
np.uint8) # Create an upper bound for skin color
skinRegion = cv2.inRange(converted, min_YCrCb,
max_YCrCb) # Create a mask with boundaries
skinMask = cv2.inRange(converted, min_YCrCb,
max_YCrCb) # Duplicate of the mask f
kernel = cv2.getStructuringElement(
cv2.MORPH_ELLIPSE,
(11, 11)) # Apply a series of errosion and dilations to the mask
#skinMask = cv2.erode(skinMask, kernel, iterations = 2) # Using an elliptical Kernel
#skinMask = cv2.dilate(skinMask, kernel, iterations = 2)
skinMask = cv2.GaussianBlur(skinMask, (3, 3),
0) # Blur the image to remove noise
skin = cv2.bitwise_and(img, img,
mask=skinMask) # Apply the mask to the frame
height, width, depth = cv_image.shape # Obtain the dimensions of the image
self.frame = cv_image.shape
hands_cascade = cv2.CascadeClassifier(
'/home/steven/ros_ws/src/test_cam/haarcascade_hand.xml')
hands = hands_cascade.detectMultiScale(
skinMask, 1.3, 5) # Detect the hands on the converted image
contours, hierarchy = cv2.findContours(
skinRegion, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE) # Find the contour on the skin detection
for i, c in enumerate(
contours): # Draw the contour on the source frame
area = cv2.contourArea(c)
if area > 10000: # Noise isn't circled
cv2.drawContours(img, contours, i, (255, 255, 0), 2)
for (x, y, z, h) in hands: # Get the coordinates of the hands
d = h / 2
self.x = x + d # Gets the center of the detected hand
self.y = y + d # Gets the center of the detected hand
cv2.circle(img, (self.x, self.y), 50, (0, 0, 255),
5) # Circle the detected hand
self.img_viewer = img
def callback_checker(self, data):
'''
This is the callback function of the checker, i.e, the thread that the checker creates once it's initialized.
The checker callback runs in the same way as the viewer callback, but its main use is to ensure that a hand
is still detected. It does so by checking the area of the contour drawn on the image, and hence, unlike the
viewer callback, the contour affects the hand detection. The contour area is however unstable, and might not
produce the best results. The skin color detection has a high range of red color as wll, making the contour
detection less stable when a red colopr is in range. Hence that is why the exposure of the checker is decreased
so as to reduce the noise colors.
'''
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
img = cv_image
converted = cv2.cvtColor(
img, cv2.COLOR_BGR2YCR_CB) # Convert image color scheme to YCrCb
min_YCrCb = np.array(
[0, 133, 77], np.uint8) # Create a lower bound for the skin color
max_YCrCb = np.array([255, 173, 127],
np.uint8) # Create an upper bound for skin color
skinRegion = cv2.inRange(converted, min_YCrCb,
max_YCrCb) # Create a mask with boundaries
skinMask = cv2.inRange(
converted, min_YCrCb,
max_YCrCb) # Duplicate of the mask for comparison
kernel = cv2.getStructuringElement(
cv2.MORPH_ELLIPSE,
(11, 11)) # Apply a series of errosion and dilations to the mask
#skinMask = cv2.erode(skinMask, kernezl, iterations = 2) # Using an elliptical Kernel
#skinMask = cv2.dilate(skinMask, kernel, iterations = 2)
skinMask = cv2.GaussianBlur(skinMask, (3, 3),
0) # Blur the image to remove noise
skin = cv2.bitwise_and(img, img,
mask=skinMask) # Apply the mask to the frame
height, width, depth = cv_image.shape
hands_cascade = cv2.CascadeClassifier(
'/home/steven/ros_ws/src/test_cam/haarcascade_hand.xml')
hands = hands_cascade.detectMultiScale(
skinMask, 1.3, 5) # Detect the hands on the converted image
for (x, y, z, h) in hands: # Get the coordinates of the hands
d = h / 2
x = x + d
y = y + d
cv2.circle(img, (x, y), 50, (0, 0, 255), 5)
contours, hierarchy = cv2.findContours(
skinRegion, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE) # Find the contour on the skin detection
self.hand_area = 0
for i, c in enumerate(
contours): # Draw the contour on the source frame
area = cv2.contourArea(c)
if area > 10000:
cv2.drawContours(img, contours, i, (255, 255, 0), 2)
self.hand_area = area # - area_1
# The following function will create a contour based on the red color scheme. This function should be enabled whenever
# a red color is detected by the checker. The red color detected will alter the hand area detected, and hence will
# detect an large area even if a hand is not in range, area corresponding to the red color. Hence the red area should be subtracted
# from the entire detected area, to obtain the actual area of the hand
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
lower_red = np.array([0, 10, 10])
upper_red = np.array([10, 240, 240])
red_mask = cv2.inRange(hsv, lower_red, upper_red)
red = cv2.bitwise_and(img, img, mask=red_mask)
cv2.imshow("Viewer Camera // Checker Camera",
np.hstack([img, self.img_viewer]))
cv2.waitKey(1)
def torque_mag(self, a):
# Gets the magnitude of the torque detected
mag_a = math.sqrt((a['force'].z * a['force'].z) +
(a['force'].y * a['force'].y) +
(a['force'].x * a['force'].x))
return mag_a
def get_starting_pos(self):
# Sets the limbs to their correct starting position
if self.limb == 'left':
self.left_arm.move_to_joint_positions(dict({
'left_e0':
-1.1339952974442922,
'left_e1':
1.2954467753692318,
'left_s0':
0.8252816638823526,
'left_s1':
0.048703890015361885,
'left_w0':
-0.14879613642488512,
'left_w1':
1.176179769111141,
'left_w2':
0.5867476513661708
}),
timeout=15.0)
self.right_arm.move_to_joint_positions(dict({
'right_e0':
1.0438739261560241,
'right_e1':
1.2797234722934063,
'right_s0':
-0.1257864246066039,
'right_s1':
-0.3171505278953093,
'right_w0':
0.3186845086831947,
'right_w1':
1.1278593742927505,
'right_w2':
-0.215907795894872
}),
timeout=15.0)
else:
self.left_arm.move_to_joint_positions(dict({
'left_e0':
-1.2475098757478127,
'left_e1':
1.1830826826566254,
'left_s0':
0.292990330486114,
'left_s1':
-0.12540292940963257,
'left_w0':
-0.024927187803137973,
'left_w1':
1.301966193717745,
'left_w2':
0.15953400194008302
}),
timeout=15.0)
self.right_arm.move_to_joint_positions(dict({
'right_e0':
1.0933448065653286,
'right_e1':
1.2609322076418101,
'right_s0':
-0.6024709544419963,
'right_s1':
-0.08053399136398422,
'right_w0':
0.2906893593042859,
'right_w1':
1.212995308020391,
'right_w2':
-0.19251458887961942
}),
timeout=15.0)
def unit_vector_to_point(self, x, y):
'''
Creates a unit vector from the camera's frame, given a pixel point from the image
'''
height, width, depth = self.frame # Obtain the dimensions of the frame
cam_info = rospy.wait_for_message("/cameras/" + self.camera_viewer +
"/camera_info",
CameraInfo,
timeout=None)
img_proc = PinholeCameraModel()
img_proc.fromCameraInfo(cam_info)
# The origin of the camera is initally set to the top left corner
# However the projectPixelTo3dRay uses the origin at the center of the camera. Hence the coordinates have to be converted
x = x - width / 2
y = height / 2 - y
# Creates a unit vector to the given point. Unit vector passes through point from the center of the camera
n = img_proc.projectPixelTo3dRay((x, y))
return n
def unit_vector_gripper_frame(self, u_vector):
'''
Converts the unit vector from the camera's frame to that of the gripper
'''
pose = self.arm_viewer.endpoint_pose()
pos_gripper = [
pose['position'].x, pose['position'].y, pose['position'].z
]
u_vector_gripper = [
u_vector[0] + self.camera_gripper_offset[0],
u_vector[1] + self.camera_gripper_offset[1],
u_vector[2] + self.camera_gripper_offset[2]
]
return u_vector_gripper
def align(self, alignment_vector=None):
'''
To ensure that a inverse kinematics will return a valid joint solution set, a first alignment is needed.
Aligning the gripper with the unit vector to the hand's position will ensure that.
'''
if alignment_vector != None:
alignment_vector = alignment_vector[:
3] # Ensure that vector is a 3x1
height, width, depth = self.frame # Gets the dimension of the image
pose = self.arm_viewer.endpoint_pose(
) # Gets the current pose of the end effector
pos = [pose['position'].x, pose['position'].y,
pose['position'].z] # Gets the position
quat = [
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
] # Gets the orientation
__matrix = self.tf.fromTranslationRotation(
pos, quat) # Rotational matrix is obtained from pos and quat
__matrix = __matrix[:3, :
3] # __matrix contain the rotational, and translational component, alongside with a last row of 0,0,0,1 i.e matrix is 4x4
# Only the rotational part is needed for alignement purposes, i.e a 3x3
def alignment_to_vec(
b, a
): # b = z-axis vector // a = alignment vector (unit vector to point)
'''
Returns the rotational matrix, that wil align the vector b to a
'''
a_dot_b = sum(imap(mul, a, b)) #np.dot(a,b)
n_a = np.array([float(a[0]), float(a[1]), float(a[2])])
n_b = np.array([float(b[0]), float(b[1]), float(b[2])])
a_x_b = np.cross(n_a, n_b)
a_x_b = np.matrix([[float(a_x_b[0])], [float(a_x_b[1])],
[float(a_x_b[2])]])
mod_a = math.sqrt((float(a[0]) * float(a[0])) +
(float(a[1]) * float(a[1])) +
(float(a[2]) * float(a[2])))
mod_b = math.sqrt((float(b[0]) * float(b[0])) +
(float(b[1]) * float(b[1])) +
(float(b[2]) * float(b[2])))
mod_a_x_b = math.sqrt((float(a_x_b[0]) * float(a_x_b[0])) +
(float(a_x_b[1]) * float(a_x_b[1])) +
(float(a_x_b[2]) * float(a_x_b[2])))
x = a_x_b / mod_a_x_b
alpha = a_dot_b / (mod_a * mod_b)
theta = math.acos(alpha)
a_matrix = np.matrix([[0,
float(-x[2]), float(x[1])],
[float(x[2]), 0,
float(-x[0])],
[float(-x[1]), float(x[0]), 0]])
rotation_matrix = np.identity(3) + (math.sin(theta) * a_matrix) + (
(1 - (math.cos(theta))) * (np.dot(a_matrix, a_matrix)))
return rotation_matrix
def create_from_rot_matrix(rot):
"""
Rotation matrix created from quaternion
Create from rotation matrix,
modified from
https://github.com/CMU-ARM/ARBT-Baxter-Nav-Task/blob/stable/scripts/Quaternion.py
"""
tr = np.trace(rot)
if (tr > 0):
s = np.sqrt(tr + 1) * 2
w = 0.25 * s
x = (rot[2, 1] - rot[1, 2]) / s
y = (rot[0, 2] - rot[2, 0]) / s
z = (rot[1, 0] - rot[0, 1]) / s
elif (rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]):
s = np.sqrt(1 + rot[0, 0] - rot[1, 1] - rot[2, 2]) * 2
w = (rot[2, 1] - rot[1, 2]) / s
x = 0.25 * s
y = (rot[0, 1] + rot[1, 0]) / s
z = (rot[0, 2] + rot[2, 0]) / s
elif (rot[1, 1] > rot[2, 2]):
s = np.sqrt(1 + rot[1, 1] - rot[0, 0] - rot[2, 2]) * 2
w = (rot[0, 2] - rot[2, 0]) / s
x = (rot[0, 1] + rot[1, 0]) / s
y = 0.25 * s
z = (rot[1, 2] + rot[2, 1]) / s
else:
s = np.sqrt(1 + rot[2, 2] - rot[1, 1] - rot[0, 0]) * 2
w = (rot[1, 0] - rot[0, 1]) / s
x = (rot[0, 2] + rot[2, 0]) / s
y = (rot[1, 2] + rot[2, 1]) / s
z = 0.25 * s
return x, y, z, w
def hamilton_product(b, a):
q = [x, y, z, w]
q[3] = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2]
q[0] = a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1]
q[1] = a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0]
q[2] = a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3]
return q
z_vector = np.dot(__matrix, np.matrix(
[[0], [0],
[1]])) # Converts the z-axis of the camera to the base frame
print "Z-vector BF: ", z_vector
rotation_matrix = alignment_to_vec(
z_vector, alignment_vector
) # Rotation matrix that aligns the z-axis to the unit vector, pointing towards the hand
x, y, z, w = create_from_rot_matrix(
rotation_matrix) # Gets the orientation of alignement
rot_quat = [x, y, z, w]
final_quat = hamilton_product(
quat, rot_quat) # Gets the final orientation of alignement
self.ik(self.limb, pos, final_quat, True) # Aligns the viewer
print "Aligned"
def pixel_translation(self, uv):
# Converts the pixel coordinates to spatial coordinates WRT the camera's frame
pose = self.arm_viewer.endpoint_pose()
xy = [pose['position'].x, pose['position'].y, pose['position'].z]
height, width, depth = self.frame #camera frame dimensions
print "\nx-pixel: ", uv[0], "\ty-pixel: ", uv[1]
cx = uv[0] # x pixel of hand
cy = uv[1] # y pixel of hand
pix_size = 0.0025 # Camera calibration (meter/pixels); Pixel size at 1 meter height of camera
h = self.z_camera #Height from hand to camera, when at vision place
x0b = xy[
0] # x position of camera in baxter's base frame, when at vision place
y0b = xy[
1] # y position of camera in baxter's base frame, when at vision place
x_camera_offset = .045 #x camera offset from center of the gripper
y_camera_offset = -.01 #y camera offset from center of the gripper
#Position of the object in the baxter's stationary base frame
x_camera = (cy - (height / 2)
) * pix_size * h - 0.045 # x-coordinate in camera's frame
y_camera = (cx - (width / 2)
) * pix_size * h + 0.01 # y-coordiante in camera's frame
return x_camera, y_camera, h
def xy_translation(self):
uv = (self.x, self.y) # pixel coordinates
if self.x != 0 and self.y != 0 and self.z_camera != None: # If a hand has been detected and a position recorded
x_camera, y_camera, h = self.pixel_translation(
uv) # Obtains the spatial x,y coordinates
self.depth_detection(float(x_camera), float(y_camera),
h) # proceeds to coordinates translation
else:
self.depth_detection(
0, 0, 0
) #If a depth hasn't been detected, proceed to the depth detection
def depth_detection(self, x, y, z):
pose = self.arm_viewer.endpoint_pose()
pos = [pose['position'].x, pose['position'].y, pose['position'].z]
quat = [
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
]
height, width, depth = self.frame #camera frame dimensions
__matrix = self.tf.fromTranslationRotation(pos,
quat) #self.orientation_cam
# If a depth has already been detected
# Proceed to coordinates translation, and use inverse kinematics to move to position
if self.z_camera != None:
print "depth detected"
z = self.z_camera
xyz = np.dot(__matrix, np.matrix([[x], [y], [z], [1]]))
print "\nx-coordinate obtained: ", xyz[
0], "\ny-coordinate obtained: ", xyz[
1], "\nz-coordinate obtained: ", xyz[2]
pos = [xyz[0], xyz[1], xyz[2]]
self.ik(self.limb, pos, self.orientation_shaker)
# Else, move the arm towards unit vector until a depth has been detected
else:
print "...Moving arm..."
__matrix = __matrix[:3, :3]
# Aligns the end effector to the detected hand before moving it
n = self.unit_vector_to_point(self.x, self.y)
if self.x > width / 2: # If self.x > widht/2, hand is above the camera
up = True # Will pass this as an argument to generate the unit vector, indicating that the hand is above
else:
up = False
u_vector_gripper = self.unit_vector_gripper_frame(n)
u_vector = np.dot(
__matrix,
np.matrix([[u_vector_gripper[0]], [u_vector_gripper[1]],
[u_vector_gripper[2]]]))
print "...Aligning arm..."
self.align(u_vector)
#self.with_check(__matrix,up)
self.without_check(__matrix, up)
def without_check(self, __matrix, up=False):
height, width, depth = self.frame #camera frame dimensions
while self.z_camera == None and self.torque < 20: # While no depth is detcted and no torque is detected
n = self.unit_vector_to_point(
width / 2,
height / 2) # Unit vector to the center of the camera
# the unit vector to the center of the camera is generated, since after alignement, the hand should be almost in line with the center
u_vector_gripper = self.unit_vector_gripper_frame(
n) # converts the unit vector to the gripper's frame
u_vector = np.dot(__matrix,
np.matrix([[u_vector_gripper[0]],
[u_vector_gripper[1]],
[u_vector_gripper[2]]
])) # converts to the base frame
if self.limb == 'left':
if up == True: # Accounts for the position of the hand, above or below the camera
# Unit vector seems to always have a negative z-component, hence if the hand is detected above the camera, the z-component is negated
u_vector[2] = -u_vector[2] # negating the z-component
elif self.limb == 'right':
if up != True: # The inverse is true for the right arm
u_vector[2] = -u_vector[2]
pose = self.arm_viewer.endpoint_pose()
pos = [
pose['position'].x + (u_vector[0] / 20),
pose['position'].y + (u_vector[1] / 20),
pose['position'].z + (u_vector[2] / 20)
] # Move the arm by increments
quat = [
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
]
self.ik(self.limb, pos, quat)
# Once depth has been detected, change orientation of gripper to that of the shaker, defined when initialized
pose = self.arm_viewer.endpoint_pose()
pos = [pose['position'].x, pose['position'].y, pose['position'].z]
if self.limb == 'left':
self.ik('left', pos, self.left_orientation_shaker,
True) # Hand-shaking position for left arm
else:
self.ik('right', pos, self.right_orientation_shaker,
True) # Hand-shaking position for right arm
print "Position reached...Moving to Instructor position"
self.follow_up() # Follow_up instructions
def with_check(self, __matrix, up):
height, width, depth = self.frame #camera frame dimensions
print self.hand_area
while self.z_camera == None and self.torque < 20 and self.hand_area > 5000: # As long as hand area is detected
if self.hand_area > 5000:
n = self.unit_vector_to_point(width / 2, height / 2)
u_vector_gripper = self.unit_vector_gripper_frame(n)
u_vector = np.dot(
__matrix,
np.matrix([[u_vector_gripper[0]], [u_vector_gripper[1]],
[u_vector_gripper[2]]]))
if self.limb == 'left':
if up == True:
u_vector[2] = -u_vector[2]
elif self.limb == 'right':
print self.limb
if up != True:
u_vector[2] = -u_vector[2]
pose = self.arm_viewer.endpoint_pose()
pos = [
pose['position'].x + (u_vector[0] / 20),
pose['position'].y + (u_vector[1] / 20),
pose['position'].z + (u_vector[2] / 20)
]
quat = [
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
]
self.ik(self.limb, pos, quat)
else:
print "No hands detected..."
return 0
if self.hand_area > 5000:
pose = self.arm_viewer.endpoint_pose()
pos = [pose['position'].x, pose['position'].y, pose['position'].z]
self.ik('left', pos, self.left_orientation_shaker, True)
print "Position reached...Moving to Instructor position"
#self.follow_up()
else:
print "No hands detected for final alignment"
def ik(self, limb, pos, quat, block=None, arm=None):
'''
This function uses inverse kinematics to calculate the joint states given a certain pose.
It also applies the joint states to the specified arms, i.e moves the arm. Arguments:
- limb : the limb which is to be moved. If not specified, limb is viewer limb
- pos,quat : pose for which joint solutions are desired
- block - if block is None, the motion of the joints will be done by set_joint_positions.
Else, it will be done by move_to_joint_positions.
set_joint_positions allows the operation to be interupted (Used when moving the arm towards the hand)
move_to_joint_positions cannot be interupted, and is used when aligning the end effector
******* CORE OF FUNCTION IS FROM /BAXTER_EXAMPLES/IK_SERVICE_CLIENT.PY********
'''
if arm == None:
arm = self.arm_viewer
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'left':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(
x=quat[0],
y=quat[1],
z=quat[2],
w=quat[3],
),
),
),
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(
x=quat[0],
y=quat[1],
z=quat[2],
w=quat[3],
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
# Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
resp.result_type)
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
#print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
# (seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
# reformat the solution arrays into a dictionary
joint_solution = dict(
zip(resp.joints[0].name, resp.joints[0].position))
if block == None:
arm.set_joint_positions(joint_solution)
else:
arm.move_to_joint_positions(joint_solution)
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
'right_w2': -0.9725438195193523,
'right_e0': -0.40727189918357737,
'right_e1': 1.161990446823201
}
neutral_left = {
'left_w0': -0.3006602344255411,
'left_w1': 0.5549175500175484,
'left_w2': 3.050704291907117,
'left_e0': 0.5161845351234418,
'left_e1': 1.1984224905354794,
'left_s0': -0.8904758473674826,
'left_s1': -0.38387869216832476
}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
####PART 1 ("what would you like today")
#can we track the person with Baxter's head?
###PART 2 (COOKIE)
Head.set_pan(head, 0.45, speed=1.0, timeout=0.0) #looks at cookie section
#lifts arm above cookies
left.move_to_joint_positions({
'left_w0': -0.177941771394708,
'left_w1': 1.131694326262464,
'left_w2': 3.0161897241796947,
'left_e0': 0.24965537322835107,
'left_e1': 0.8038059328519568,
def main():
"""
Main Script
"""
while not rospy.is_shutdown():
planner = PathPlanner("right_arm")
limb = Limb("right")
joint_angles = limb.joint_angles()
print(joint_angles)
camera_angle = {
'right_j6': 1.72249609375,
'right_j5': 0.31765625,
'right_j4': -0.069416015625,
'right_j3': 1.1111962890625,
'right_j2': 0.0664150390625,
'right_j1': -1.357775390625,
'right_j0': -0.0880478515625
}
limb.set_joint_positions(camera_angle)
limb.move_to_joint_positions(camera_angle,
timeout=15.0,
threshold=0.008726646,
test=None)
#drawing_angles = {'right_j6': -2.00561328125, 'right_j5': -1.9730205078125, 'right_j4': 1.5130146484375, 'right_j3': -1.0272568359375, 'right_j2': 1.24968359375, 'right_j1': -0.239498046875, 'right_j0': 0.4667275390625}
#print(joint_angles)
#drawing_angles = {'right_j6': -1.0133310546875, 'right_j5': -1.5432158203125, 'right_j4': 1.4599892578125, 'right_j3': -0.04361328125, 'right_j2': 1.6773486328125, 'right_j1': 0.019876953125, 'right_j0': 0.4214736328125}
drawing_angles = {
'right_j6': 1.9668515625,
'right_j5': 1.07505859375,
'right_j4': -0.2511611328125,
'right_j3': 0.782650390625,
'right_j2': 0.25496875,
'right_j1': -0.3268349609375,
'right_j0': 0.201146484375
}
raw_input("Press <Enter> to take picture: ")
waypoints_abstract = vision()
print(waypoints_abstract)
#ar coordinate :
x = 0.461067
y = -0.235305
#first get the solution of the maze
#solutionpoints = [(0,0),(-0.66,0.16), (-0.7, 0.4)]
# Make sure that you've looked at and understand path_planner.py before starting
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
r = rospy.Rate(10)
#find trans from
while not rospy.is_shutdown():
try:
trans = tfBuffer.lookup_transform('base', 'ar_marker_0',
rospy.Time()).transform
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
if tf2_ros.LookupException:
print("lookup")
elif tf2_ros.ConnectivityException:
print("connect")
elif tf2_ros.ExtrapolationException:
print("extra")
# print("not found")
pass
r.sleep()
mat = as_transformation_matrix(trans)
point_spaces = []
for point in waypoints_abstract:
# for point in solutionpoints:
point = np.array([point[0], point[1], 0, 1])
point_space = np.dot(mat, point)
point_spaces.append(point_space)
print(point_spaces)
length_of_points = len(point_spaces)
raw_input("Press <Enter> to move the right arm to drawing position: ")
limb.set_joint_positions(drawing_angles)
limb.move_to_joint_positions(drawing_angles,
timeout=15.0,
threshold=0.008726646,
test=None)
##
## Add the obstacle to the planning scene here
##
#add obstacle
size = [0.78, 1.0, 0.05]
name = "box"
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.77
pose.pose.position.y = 0.0
pose.pose.position.z = -0.18
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
planner.add_box_obstacle(size, name, pose)
#limb.set_joint_positions( drawing_angles)
#limb.move_to_joint_positions( drawing_angles, timeout=15.0, threshold=0.008726646, test=None)
#starting position
x, y, z = 0.67, 0.31, -0.107343
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
waypoint = []
for point in point_spaces:
goal_1.pose.position.x = point[0]
goal_1.pose.position.y = point[1]
goal_1.pose.position.z = -0.113343 #set this value when put different marker
waypoint.append(copy.deepcopy(goal_1.pose))
plan, fraction = planner.plan_straight(waypoint, [])
print(fraction)
raw_input("Press <Enter> to move the right arm to draw: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
raw_input("Press <Enter> to start again: ")
from baxter_interface import Gripper, Head, Limb
rospy.init_node('run_condition')
voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10)
time.sleep(1)
rightg = Gripper('right')
rightg.set_holding_force(50)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
# neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
# neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658}
# right.move_to_joint_positions(neutral_right)
# left.move_to_joint_positions(neutral_left)
def wait():
raw_input("Press Enter to continue...")
class BaxterController():
def __init__(self, limb_name, trajectory, offset, radius):
# crate a limb instance control the baxter arm
self.limb = Limb(limb_name)
# numpy joint angle trajectory of nx7 shape
self.trajectory = trajectory
# index variable to keep track of current row in numpy array
self.trajectory_index = 0
# store the joint names since
# it will be used later while commanding the limb
self.joint_names = self.limb.joint_names()
# define a service called 'move_arm_to_waypoint'
self.move_arm_to_waypoint_service = rospy.Service(
'move_arm_to_waypoint', Trigger, self.handle_move_arm_to_waypoint)
# define another service called 'get_ee_pose'
self.get_ee_position_service = rospy.Service(
'get_ee_position', GetEEPosition,
self.handle_get_ee_position_service)
# flag to set when trajectory is finished
self.trajectory_finished = False
# define 4x4 transformation for marker wrt end-effector
# considering no rotation in 'marker_wrt_ee' transformation matrix
self.marker_wrt_ee = np.eye(4)
self.marker_wrt_ee[2, -1] = offset + radius # in z direction only
def spin(self):
# let the ros stay awake and serve the request
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not self.trajectory_finished:
rate.sleep()
# wait some time before stopping the node so that the service request returns if any
rospy.sleep(1)
self.get_ee_position_service.shutdown()
self.move_arm_to_waypoint_service.shutdown()
rospy.logdebug('Shutting down the baxter controller node')
rospy.signal_shutdown('User requested')
def handle_get_ee_position_service(self, request):
# create a response object for the 'GetEEPose' service
ee_pose = self.limb.endpoint_pose()
# get rotation matrix from quaternion. 'quaternion_matrix'
# returns 4x4 HTM with translation set to sero
ee_wrt_robot = quaternion_matrix(ee_pose['orientation'])
ee_wrt_robot[:-1, -1] = ee_pose['position'] # update the translation
# marker wrt robot = marker_wrt_ee * ee_wrt_robot
marker_wrt_robot = ee_wrt_robot.dot(self.marker_wrt_ee)
response = GetEEPositionResponse()
response.position.x = marker_wrt_robot[0, -1]
response.position.y = marker_wrt_robot[1, -1]
response.position.z = marker_wrt_robot[2, -1]
return response
def handle_move_arm_to_waypoint(self, request):
# create a response object for the trigger service
response = TriggerResponse()
# check if the trajectory finished
trajectory_finished = self.trajectory_index >= self.trajectory.shape[0]
# if trajectory isn't finished
if not trajectory_finished:
# get the latest joint values from given trajectory
joint_values = trajectory[self.trajectory_index, :]
# create command, i.e., a dictionary of joint names and values
command = dict(zip(self.joint_names, joint_values))
# move the limb to given joint angle
try:
self.limb.move_to_joint_positions(command)
response.message = 'Successfully moved arm to the following waypoint %s' % command
except rospy.exceptions.ROSException:
response.message = 'Error while moving arm to the following waypoint %s' % command
finally:
# increment the counter
self.trajectory_index += 1
else:
response.message = 'Arm trajectory is finished already'
# set the success parameter of the response object
response.success = trajectory_finished
# set the flag just before returning from the function so that it is
# almost certain that the service request is returned successfully
self.trajectory_finished = trajectory_finished
return response
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201}
neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
####PART 1 ("what would you like today")
#can we track the person with Baxter's head?
###PART 2 (COOKIE)
Head.set_pan(head, 0.45, speed = 1.0, timeout = 0.0) #looks at cookie section
#lifts arm above cookies
left.move_to_joint_positions({'left_w0': -0.177941771394708, 'left_w1': 1.131694326262464, 'left_w2': 3.0161897241796947, 'left_e0': 0.24965537322835107, 'left_e1': 0.8038059328519568, 'left_s0': -0.7320923310183137, 'left_s1': -0.4571262747898533}
)
'right_w1': 0.7037136864424336,
'right_w2': -0.28033498898605935,
'right_e0': -0.16566992509162468,
'right_e1': 1.3077186216723151
}
neutral_left = {
'left_w0': -0.15339807878854136,
'left_w1': 0.34552917247118947,
'left_w2': 3.0158062289827234,
'left_e0': 0.18676216092504913,
'left_e1': 1.5715633171886063,
'left_s0': -0.5836796897904,
'left_s1': -0.6845389265938658
}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
head.set_pan(0.0, speed=0.8, timeout=0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
def wait():
raw_input("Press Enter to continue...")
def send_voice(msg):
print "Baxter: %s" % index_to_string[msg]
voice_pub.publish(data=msg)
try:
tucker.supervised_tuck()
except Exception, e:
rospy.logerr(e.strerror)
rospy.logerr("Failed to return arms to untucked position.")
return False
rospy.loginfo("Arms in untucked position.")
rospy.loginfo("Moving arms to neutral position.")
left_arm_neutral = rospy.get_param(LEFT_ARM_NEUTRAL)
right_arm_neutral = rospy.get_param(RIGHT_ARM_NEUTRAL)
arm_l = Limb("left")
arm_r = Limb("right")
arm_l.move_to_joint_positions(left_arm_neutral)
arm_r.move_to_joint_positions(right_arm_neutral)
return True
def callback(self, request=True):
"""
Checks the status of the grasping components.
:param request: request from brain to check the handling
:return: True if all checks are good and 'False' otherwise
"""
# 1. Enable Arms
if not self.enable_arms():
return False
