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 vision_server():
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0,10)*-1
self.history_y = np.arange(0,10)*-1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400,640,3), np.uint8)
cv2.imshow('image',self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
#print np.dot(self.J,self.jointVelocity)
def _read_distance(self,data):
self.distance = data.range
def _set_threshold(self):
cv2.createTrackbar('Min R(H)','image',0,255,nothing)
cv2.createTrackbar('Max R(H)','image',255,255,nothing)
cv2.createTrackbar('Min G(S)','image',0,255,nothing)
cv2.createTrackbar('Max G(S)','image',255,255,nothing)
cv2.createTrackbar('Min B(V)','image',0,255,nothing)
cv2.createTrackbar('Max B(V)','image',255,255,nothing)
def get_joint_velocity(self):
cmd = {}
velocity_list = np.asarray([1,2,3,4,5,6,7],np.float32)
for idx, name in enumerate(self.control_joint_names):
v = self.control_arm.joint_velocity( self.control_joint_names[idx])
velocity_list[idx] = v
cmd[name] = v
return velocity_list
def list_to_dic(self,ls):
cmd = {}
for idx, name in enumerate(self.control_joint_names):
v = ls.item(idx)
cmd[name] = v
return cmd
def PID(self):
Kp = 0.5
vy = -Kp*self.dx
vx = -Kp*self.dy
return vx,vy
def _on_camera(self, data):
self.busy = True
self.framenumber += 1
index = self.framenumber % 10
cv2.namedWindow('image')
cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8")
np_image = np.asarray(cv_image)
image_after_process, mask = self.image_process(np_image)
self.history_x[index] = self.dx*100
self.history_y[index] = self.dy*100
self.avg_dx = np.average(self.history_x)/100
self.avg_dy = np.average(self.history_y)/100
# if abs(self.dx)<0.01 and abs(self.dy)<0.01:
# self.found = True
# else:
# self.found = False
vx, vy = self.PID()
self.v_end = np.asarray([(1-((abs(vx)+abs(vy))*5))*0.03,vy,vx,0,0,0],np.float32)
#self.v_end = np.asarray([0,0,0,0,0,0.05],np.float32)
self.v_joint = np.dot(self.J_inv,self.v_end)
self.cmd = self.list_to_dic(self.v_joint)
self.busy = False
cv2.imshow('image',image_after_process)
cv2.waitKey(1)
def image_process(self,img):
# min_r = cv2.getTrackbarPos('Min R(H)','image')
# max_r = cv2.getTrackbarPos('Max R(H)','image')
# min_g = cv2.getTrackbarPos('Min G(S)','image')
# max_g = cv2.getTrackbarPos('Max G(S)','image')
# min_b = cv2.getTrackbarPos('Min B(V)','image')
# max_b = cv2.getTrackbarPos('Max B(V)','image')
# min_r = 0
# max_r = 57
# min_g = 87
# max_g = 181
# min_b = 37
# max_b = 70
min_r = 0
max_r = 58
min_g = 86
max_g = 255
min_b = 0
max_b = 255
min_color = (min_r, min_g, min_b)
max_color = (max_r, max_g, max_b)
#img = cv2.flip(img, flipCode = -1)
hsv_img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, min_color, max_color)
result = cv2.bitwise_and(img, img, mask = mask)
mask_bool = np.asarray(mask, bool)
label_img = morphology.remove_small_objects(mask_bool, 1000, connectivity = 1)
objects = measure.regionprops(label_img)
if objects != []:
self.found = True
target = objects[0]
box = target.bbox
cv2.rectangle(img,(box[1],box[0]),(box[3],box[2]),(0,255,0),3)
dx_pixel=target.centroid[1]-self.centerx
dy_pixel=target.centroid[0]-self.centery
dx=dx_pixel*self.coefx
dy=dy_pixel*self.coefy
angle = target.orientation
cv2.circle(img,(int(target.centroid[1]),int(target.centroid[0])),10,(0,0,255),-1)
self.dx = dx
self.dy = dy
#print self.dx,self.dy
else:
self.found = False
self.dx = 0
self.dy = 0
return img, mask_bool
def handle_vision_req(self, req):
#resp = VisionResponse(req.requestID, 0, self.avg_dx, self.avg_dy)
self.finish = 0
if self.found == True:
if self.distance > 0.07:
self.control_arm.set_joint_velocities(self.cmd)
#rospy.sleep(0.02)
else:
self.finish = 1
resp = VisionResponse(req.requestID, self.finish, self.dx, self.dy)
return resp
def clean_shutdown(self):
print "Server finished"
cv2.imwrite('box_img.png', self.blank_image)
rospy.signal_shutdown("Done")
sys.exit()
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = left_arm.joint_angles()
velocities = left_arm.joint_velocities()
force = convert_dict_to_list('left', left_arm.joint_efforts())
# Initial states
q_previous = positions # Starting joint angles
q_dot_previous = velocities # Starting joint velocities
x_previous = kin.forward_position_kinematics(
) # Starting end effector position
x_dot_previous = np.zeros((6, 1))
# Set model parameters:
m = 1
K = 0.2
C = 15
B = 12
while True:
'''
Following code breaks loop upon user input (enter key)
'''
try:
stdin = sys.stdin.read()
if "\n" in stdin or "\r" in stdin:
return_to_init(joints, 'left')
break
except IOError:
pass
# Gather Jacobian information:
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
# Extract sensor data:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
# Information is published at 100Hz by default (every 10ms=.01sec)
time_step = 0.01
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left', left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
force_mag = np.linalg.norm(force)
print(force_mag)
if (force_mag < 28): # Add deadzone
continue
else:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities, [7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_ref = np.reshape(x, [6, 1]) # Reference position
x_ref_dot = J * velocities # Reference velocities
t_stop = 10
t_end = time.time() + t_stop # Loop timer (in seconds)
# Initial plot parameters
time_plot_cum = 0
time_vec = [time_plot_cum]
pos_vec = [x_ref[1][0]]
# For integral control
x_ctrl_cum = 0
time_cum = 0.00001 # Prevent divide by zero
x_ctrl_prev = 0 # Initial for derivative ctrl
while time.time() < t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
joints = left_arm.joint_names()
positions = convert_dict_to_list('left',
left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities,
[7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_current = np.reshape(x, [6, 1])
x_dot_current = J * velocities
# Only interested in y-axis:
x_dot_current[0] = 0
#x_dot_current[1]=0
x_dot_current[2] = 0
x_dot_current[3] = 0
x_dot_current[4] = 0
x_dot_current[5] = 0
x_ddot = derivative(x_dot_previous, x_dot_current, time_step)
# Model goes here
f = J_T_PI * force # spacial force
# Only interested in y-axis:
f[0] = [0]
#f[1]=[0]
f[2] = [0]
f[3] = [0]
f[4] = [0]
f[5] = [0]
x_des = ((B * f - m * x_ddot - C *
(x_dot_current - x_ref_dot)) /
K) + x_ref # Spring with damper
# Control robot
Kp = 0.0004
Ki = 0.00000022
Kd = 0.0000005
x_ctrl = x_current - x_des
# Only interested in y-axis:
x_ctrl[0] = 0
#x_ctrl[1]=0
x_ctrl[2] = 0
x_ctrl[3] = 0
x_ctrl[4] = 0
x_ctrl[5] = 0
q_dot_ctrl = J_T * np.linalg.inv(J * J_T + np.identity(6)) * (
-Kp * x_ctrl - Ki * (x_ctrl_cum) - Kd *
(x_ctrl_prev - x_ctrl) / time_step)
q_dot_ctrl_list = convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict = convert_list_to_dict(joints, q_dot_ctrl_list)
left_arm.set_joint_velocities(
q_dot_ctrl_dict) # Velocity control function
# Update plot info
time_cum += .05
time_vec.append(time_cum)
pos_vec.append(x_current[1][0])
# Update integral control parameters
x_ctrl_cum += x_ctrl * time_step
# Update derivative control parameters
x_ctrl_prev = x_ctrl
# Update values before next loop
x_previous = x_current # Updates previous position for next loop iteration
x_dot_previous = x_dot_current # Updates previous velocity for next loop iteration
print(time_vec)
print(pos_vec)
plt.plot(time_vec, pos_vec)
plt.title('Position over time')
plt.xlabel('Time (sec)')
plt.ylabel('Position')
plt.show()
stop_joints(q_dot_ctrl_dict)
left_arm.set_joint_velocities(q_dot_ctrl_dict)
time.sleep(1)
break
class vision_server():
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0, 10) * -1
self.history_y = np.arange(0, 10) * -1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1 / (526 - 369)
self.coefy = 0.1 / (237 - 90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400, 640, 3), np.uint8)
cv2.imshow('image', self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision,
self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image,
self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range,
self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
#print np.dot(self.J,self.jointVelocity)
def _read_distance(self, data):
self.distance = data.range
def _set_threshold(self):
cv2.createTrackbar('Min R(H)', 'image', 0, 255, nothing)
cv2.createTrackbar('Max R(H)', 'image', 255, 255, nothing)
cv2.createTrackbar('Min G(S)', 'image', 0, 255, nothing)
cv2.createTrackbar('Max G(S)', 'image', 255, 255, nothing)
cv2.createTrackbar('Min B(V)', 'image', 0, 255, nothing)
cv2.createTrackbar('Max B(V)', 'image', 255, 255, nothing)
def get_joint_velocity(self):
cmd = {}
velocity_list = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32)
for idx, name in enumerate(self.control_joint_names):
v = self.control_arm.joint_velocity(self.control_joint_names[idx])
velocity_list[idx] = v
cmd[name] = v
return velocity_list
def list_to_dic(self, ls):
cmd = {}
for idx, name in enumerate(self.control_joint_names):
v = ls.item(idx)
cmd[name] = v
return cmd
def PID(self):
Kp = 0.5
vy = -Kp * self.dx
vx = -Kp * self.dy
return vx, vy
def _on_camera(self, data):
self.busy = True
self.framenumber += 1
index = self.framenumber % 10
cv2.namedWindow('image')
cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8")
np_image = np.asarray(cv_image)
image_after_process, mask = self.image_process(np_image)
self.history_x[index] = self.dx * 100
self.history_y[index] = self.dy * 100
self.avg_dx = np.average(self.history_x) / 100
self.avg_dy = np.average(self.history_y) / 100
# if abs(self.dx)<0.01 and abs(self.dy)<0.01:
# self.found = True
# else:
# self.found = False
vx, vy = self.PID()
self.v_end = np.asarray(
[(1 - ((abs(vx) + abs(vy)) * 5)) * 0.03, vy, vx, 0, 0, 0],
np.float32)
#self.v_end = np.asarray([0,0,0,0,0,0.05],np.float32)
self.v_joint = np.dot(self.J_inv, self.v_end)
self.cmd = self.list_to_dic(self.v_joint)
self.busy = False
cv2.imshow('image', image_after_process)
cv2.waitKey(1)
def image_process(self, img):
# min_r = cv2.getTrackbarPos('Min R(H)','image')
# max_r = cv2.getTrackbarPos('Max R(H)','image')
# min_g = cv2.getTrackbarPos('Min G(S)','image')
# max_g = cv2.getTrackbarPos('Max G(S)','image')
# min_b = cv2.getTrackbarPos('Min B(V)','image')
# max_b = cv2.getTrackbarPos('Max B(V)','image')
# min_r = 0
# max_r = 57
# min_g = 87
# max_g = 181
# min_b = 37
# max_b = 70
min_r = 0
max_r = 58
min_g = 86
max_g = 255
min_b = 0
max_b = 255
min_color = (min_r, min_g, min_b)
max_color = (max_r, max_g, max_b)
#img = cv2.flip(img, flipCode = -1)
hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, min_color, max_color)
result = cv2.bitwise_and(img, img, mask=mask)
mask_bool = np.asarray(mask, bool)
label_img = morphology.remove_small_objects(mask_bool,
1000,
connectivity=1)
objects = measure.regionprops(label_img)
if objects != []:
self.found = True
target = objects[0]
box = target.bbox
cv2.rectangle(img, (box[1], box[0]), (box[3], box[2]), (0, 255, 0),
3)
dx_pixel = target.centroid[1] - self.centerx
dy_pixel = target.centroid[0] - self.centery
dx = dx_pixel * self.coefx
dy = dy_pixel * self.coefy
angle = target.orientation
cv2.circle(img, (int(target.centroid[1]), int(target.centroid[0])),
10, (0, 0, 255), -1)
self.dx = dx
self.dy = dy
#print self.dx,self.dy
else:
self.found = False
self.dx = 0
self.dy = 0
return img, mask_bool
def handle_vision_req(self, req):
#resp = VisionResponse(req.requestID, 0, self.avg_dx, self.avg_dy)
self.finish = 0
if self.found == True:
if self.distance > 0.07:
self.control_arm.set_joint_velocities(self.cmd)
#rospy.sleep(0.02)
else:
self.finish = 1
resp = VisionResponse(req.requestID, self.finish, self.dx, self.dy)
return resp
def clean_shutdown(self):
print "Server finished"
cv2.imwrite('box_img.png', self.blank_image)
rospy.signal_shutdown("Done")
sys.exit()
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
# Initial states
q_previous=positions # Starting joint angles
q_dot_previous=velocities # Starting joint velocities
x_previous=kin.forward_position_kinematics() # Starting end effector position
x_dot_previous=np.zeros((6,1))
# Set model parameters:
C=50
B=1
f_des=np.reshape(np.array([0,15,0,0,0,0]),[6,1])
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
x=kin.forward_position_kinematics()
x=x[:-1]
x_ref=np.reshape(x,[6,1]) # Reference position
x_ref_dot=J*velocities # Reference velocities
t_stop=15
t_end=time.time()+t_stop # Loop timer (in seconds)
# Initial plot parameters
time_cum=0
time_vec=[time_cum]
f=J_T_PI*force
force_vec=[convert_mat_to_list(f[1])[0]]
while time.time()<t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
x=kin.forward_position_kinematics()
x=x[:-1]
x_current=np.reshape(x,[6,1])
x_dot_current=J*velocities
# Only interested in y-axis:
x_dot_current[0]=0
#x_dot_current[1]=0
x_dot_current[2]=0
x_dot_current[3]=0
x_dot_current[4]=0
x_dot_current[5]=0
# Model goes here
f=J_T_PI*force # spacial force
# Only interested in y-axis:
f[0]=[0]
#f[1]=[0]
f[2]=[0]
f[3]=[0]
f[4]=[0]
f[5]=[0]
x_dot_des=-B*(f_des+f)/C # Impedance control
# Control robot
q_dot_ctrl=J_PI*x_dot_des # Use this for damper system
q_dot_ctrl=np.multiply(q_dot_ctrl,np.reshape(np.array([1,0,0,0,0,0,0]),[7,1])) # y-axis translation corresponds to first shoulder joint rotation
q_dot_ctrl_list=convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict=convert_list_to_dict(joints,q_dot_ctrl_list)
left_arm.set_joint_velocities(q_dot_ctrl_dict) # Velocity control function
# Update values before next loop
x_previous=x_current # Updates previous position for next loop iteration
x_dot_previous=x_dot_current # Updates previous velocity for next loop iteration
# Update plot info
time_cum+=.05
time_vec.append(time_cum)
force_vec.append(convert_mat_to_list(f[1])[0])
print(time_vec)
print(force_vec)
plt.plot(time_vec,force_vec)
plt.title('Force applied over time')
plt.xlabel('Time (sec)')
plt.ylabel('Force (N)')
plt.show()
time.sleep(1)
