def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]]
def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
]]
def mover_setup(self):
limb = Limb("right")
# Right arm planner
self.planner = PathPlanner("right_arm")
# Left arm planner
self.planner_left = PathPlanner("left_arm")
place_holder = {'images': [], 'camera_infos': []}
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
# Controller for right arm
self.c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
self.orien_const = OrientationConstraint()
self.orien_const.link_name = "right_gripper";
self.orien_const.header.frame_id = "base";
self.orien_const.orientation.y = -1.0;
self.orien_const.absolute_x_axis_tolerance = 0.1;
self.orien_const.absolute_y_axis_tolerance = 0.1;
self.orien_const.absolute_z_axis_tolerance = 0.1;
self.orien_const.weight = 1.0;
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = self.box.pose.position.x
box.pose.position.y = self.box.pose.position.y
# box.pose.position.z = self.box.pose.position.z
box.pose.position.z = self.conveyor_z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
self.planner.add_box_obstacle((100, 100, 0.00001), "box", box)
# Controller for left arm
self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
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
def move(x, y, z, box_in):
planner = PathPlanner("right_arm")
#TODO: Add constraint restricting z > 0.
#Wait for the IK service to become available
#rospy.wait_for_service('compute_ik')
#rospy.init_node('service_query')
place_holder = {'images': [], 'camera_infos': []}
#rospy.Subscriber("/cameras/head_camera/image", Image, lambda x: place_holder['images'].append(x))
#rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo, lambda x: place_holder['camera_infos'].append(x))
limb = Limb("right")
#print(limb)
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
# while not rospy.is_shutdown() and count < 3:
# #calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point
# '''
# /cameras/head_camera/camera_info
# /cameras/head_camera/camera_info_std
# /cameras/head_camera/image
# '''
# count += 1
# raw_input("calibrate point " + str(count))
# pos = limb.endpoint_pose()
# calibration_points.append({})
# calibration_points[-1]['world_coordinates'] = (pos['position'].x, pos['position'].y, pos['position'].z)
# #calibration_points[-1]['camera_info'] = place_holder['camera_infos'][-1]
# #calibration_points[-1]['image'] = place_holder['images'][-1]
# for i in calibration_points:
# print(i['world_coordinates'])
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
## Add the controller here! IF you uncomment below, you should get the checkoff!
c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = box_in.pose.position.x
box.pose.position.y = box_in.pose.position.y
box.pose.position.z = box_in.pose.position.z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
planner.add_box_obstacle((3, 4, 0.10), "box", box)
# planner.add_box_obstacle((box_in.pose.position.x, box_in.pose.position.y, box_in.pose.position.z), "box", box)
# ree = input("some text")
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
_x, _y, _z = .67, -.2, .3
#x, y, z = .95, .11, .02
#x, y, z = .7, -.4, .2
print(0)
else:
raise Exception("not configured for sawyer yet.")
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
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_1, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move above point \n')
except Exception as e:
print(e)
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
#x, y, z = .67, -.2, 0
# x, y, z = .79, .04, -.04
pass
else:
raise Exception("not configured for sawyer yet.")
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = x
goal_2.pose.position.y = y
goal_2.pose.position.z = z
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_2, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 2: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move to point \n')
except Exception as e:
print(e)
traceback.print_exc()
else:
break
tracker.busy = True
done = False
ik()
#print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
tracker.track()
if tracker.busy == False:
tracker.gripper.close()
rospy.sleep(0.5)
arm = Limb("right")
ep_position = arm.endpoint_pose()['position']
ep_orientation = arm.endpoint_pose()['orientation']
place(right,ep_position,ep_orientation,pr,pr_place)
tracker.gripper.open()
rospy.sleep(0.5)
pr_place.position.y += 0.2
move(right,pr_place)
#move(right,pr_init)
print "I am done"
done = True
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
print ("Program finished")
def main():
planner = PathPlanner("right_arm")
#Wait for the IK service to become available
#rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
place_holder = {'images': [], 'camera_infos': []}
rospy.Subscriber("/cameras/head_camera/image", Image,
lambda x: place_holder['images'].append(x))
rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo,
lambda x: place_holder['camera_infos'].append(x))
limb = Limb("right")
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
while not rospy.is_shutdown() and count < 3:
#calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point
'''
/cameras/head_camera/camera_info
/cameras/head_camera/camera_info_std
/cameras/head_camera/image
'''
count += 1
raw_input("calibrate point " + str(count))
pos = limb.endpoint_pose()
calibration_points.append({})
calibration_points[-1]['world_coordinates'] = (pos['position'].x,
pos['position'].y,
pos['position'].z)
calibration_points[-1]['camera_info'] = place_holder['camera_infos'][
-1]
calibration_points[-1]['image'] = place_holder['images'][-1]
# for i in calibration_points:
# print(i['world_coordinates'])
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
## Add the controller here! IF you uncomment below, you should get the checkoff!
c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
for i in range(10):
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
x, y, z = .67, -.2, .108
else:
raise Exception("not configured for sawyer yet.")
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
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_1, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move above point \n')
except Exception as e:
print e
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
x, y, z = .67, -.2, 0
else:
raise Exception("not configured for sawyer yet.")
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = x
goal_2.pose.position.y = y
goal_2.pose.position.z = z
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_2, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move to point \n')
except Exception as e:
print e
traceback.print_exc()
else:
break
class Baxter(object):
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def calibrate(self):
self._left_gripper.calibrate()
self._left_gripper_max = self._left_gripper.position()
self._right_gripper.calibrate()
self._right_gripper_max = self._right_gripper.position()
@property
def left_gripper_max(self):
return self._left_gripper_max
@property
def right_gripper_max(self):
return self._right_gripper_max
@property
def left_gripper(self):
return self._left_gripper.position()
@left_gripper.setter
def left_gripper(self, position):
self._left_gripper.command_position(position)
@property
def right_gripper(self):
return self._right_gripper.position()
@right_gripper.setter
def right_gripper(self, position):
self._right_gripper.command_position(position)
def set_left_joints(self, angles):
joints = self._left.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._left.set_joint_positions(joints)
def set_right_joints(self, angles):
joints = self._right.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._right.set_joint_positions(joints)
def reset_limb(self, side):
angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()}
self.enable_check()
self._limbs[side].move_to_joint_positions(angles)
def enable_check(self):
# Sometimes robot is disabled due to another program resetting state
if not self._baxter_state.state().enabled:
self._baxter_state.enable()
@property
def joints(self):
joints = {
limb: joint.joint_angles()
for limb, joint in self._limbs.iteritems()
}
return joints
@property
def enabled(self):
return self._baxter_state.state().enabled
@property
def left_s0(self):
return self._left.joint_angle('left_s0')
@left_s0.setter
def left_s0(self, angle):
self.set_left_joints({'left_s0': angle})
@property
def left_s1(self):
return self._left.joint_angle('left_s1')
@left_s1.setter
def left_s1(self, angle):
self.set_left_joints({'left_s1': angle})
@property
def left_e0(self):
return self._left.joint_angle('left_e0')
@left_e0.setter
def left_e0(self, angle):
self.set_left_joints({'left_e0': angle})
@property
def left_e1(self):
return self._left.joint_angle('left_e1')
@left_e1.setter
def left_e1(self, angle):
self.set_left_joints({'left_e1': angle})
@property
def left_w0(self):
return self._left.joint_angle('left_w0')
@left_w0.setter
def left_w0(self, angle):
self.set_left_joints({'left_w0': angle})
@property
def left_w1(self):
return self._left.joint_angle('left_w1')
@left_w1.setter
def left_w1(self, angle):
self.set_left_joints({'left_w1': angle})
@property
def left_w2(self):
return self._left.joint_angle('left_w2')
@left_w2.setter
def left_w2(self, angle):
self.set_left_joints({'left_w2': angle})
@property
def right_s0(self):
return self._right.joint_angle('right_s0')
@right_s0.setter
def right_s0(self, angle):
self.set_right_joints({'right_s0': angle})
@property
def right_s1(self):
return self._right.joint_angle('right_s1')
@right_s1.setter
def right_s1(self, angle):
self.set_right_joints({'right_s1': angle})
@property
def right_e0(self):
return self._right.joint_angle('right_e0')
@right_e0.setter
def right_e0(self, angle):
self.set_right_joints({'right_e0': angle})
@property
def right_e1(self):
return self._right.joint_angle('right_e1')
@right_e1.setter
def right_e1(self, angle):
self.set_right_joints({'right_e1': angle})
@property
def right_w0(self):
return self._right.joint_angle('right_w0')
@right_w0.setter
def right_w0(self, angle):
self.set_right_joints({'right_w0': angle})
@property
def right_w1(self):
return self._right.joint_angle('right_w1')
@right_w1.setter
def right_w1(self, angle):
self.set_right_joints({'right_w1': angle})
@property
def right_w2(self):
return self._right.joint_angle('right_w2')
@right_w2.setter
def right_w2(self, angle):
self.set_right_joints({'right_w2': angle})
@property
def left_position(self):
return self._left.endpoint_pose()['position']
@property
def left_position_x(self):
return self.left_position.x
@left_position_x.setter
def left_position_x(self, point):
self.set_left_pose(position={'x': point})
@property
def left_position_y(self):
return self.left_position.y
@left_position_y.setter
def left_position_y(self, point):
self.set_left_pose(position={'y': point})
@property
def left_position_z(self):
return self.left_position.z
@left_position_z.setter
def left_position_z(self, point):
self.set_left_pose(position={'z': point})
@property
def left_orientation(self):
return self._left.endpoint_pose()['orientation']
@property
def left_orientation_x(self):
return self.left_orientation.x
@left_orientation_x.setter
def left_orientation_x(self, point):
self.set_left_pose(orientation={'x': point})
@property
def left_orientation_y(self):
return self.left_orientation.y
@left_orientation_y.setter
def left_orientation_y(self, point):
self.set_left_pose(orientation={'y': point})
@property
def left_orientation_z(self):
return self.left_orientation.z
@left_orientation_z.setter
def left_orientation_z(self, point):
self.set_left_pose(orientation={'z': point})
@property
def left_orientation_w(self):
return self.left_orientation.w
@left_orientation_w.setter
def left_orientation_w(self, point):
self.set_left_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_left_pose(self, position={}, orientation={}):
pos = {
'x': self.left_position_x,
'y': self.left_position_y,
'z': self.left_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.left_orientation_x,
'y': self.left_orientation_y,
'z': self.left_orientation_z,
'w': self.left_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._left_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_left_joints(pos)
else:
print 'nothing'
#print self.joints
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_position_x(self):
return self.right_position.x
@right_position_x.setter
def right_position_x(self, point):
self.set_right_pose(position={'x': point})
@property
def right_position_y(self):
return self.right_position.y
@right_position_y.setter
def right_position_y(self, point):
self.set_right_pose(position={'y': point})
@property
def right_position_z(self):
return self.right_position.z
@right_position_z.setter
def right_position_z(self, point):
self.set_right_pose(position={'z': point})
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
@property
def right_orientation_x(self):
return self.right_orientation.x
@right_orientation_x.setter
def right_orientation_x(self, point):
self.set_right_pose(orientation={'x': point})
@property
def right_orientation_y(self):
return self.right_orientation.y
@right_orientation_y.setter
def right_orientation_y(self, point):
self.set_right_pose(orientation={'y': point})
@property
def right_orientation_z(self):
return self.right_orientation.z
@right_orientation_z.setter
def right_orientation_z(self, point):
self.set_right_pose(orientation={'z': point})
@property
def right_orientation_w(self):
return self.right_orientation.w
@right_orientation_w.setter
def right_orientation_w(self, point):
self.set_right_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_right_pose(self, position={}, orientation={}):
pos = {
'x': self.right_position_x,
'y': self.right_position_y,
'z': self.right_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.right_orientation_x,
'y': self.right_orientation_y,
'z': self.right_orientation_z,
'w': self.right_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._right_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_right_joints(pos)
@property
def head_position(self):
return self._head.pan()
@head_position.setter
def head_position(self, position):
self._head.set_pan(position)
def take_sample():
rospy.init_node('Take_sample', anonymous=True)
# rospy.Subscriber('/tf', TFMessage, PoseCallback)
# rospy.Subscriber('/camera/depth/image_rect_raw', Image, ImgCallback)
right_arm = Limb('right')
left_arm = Limb('left')
cnt = 0
flag_cap = 0
print('start')
while not rospy.is_shutdown():
# # show video stream
# Wait for a coherent pair of frames: color
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
if not color_frame:
continue
# Convert images to numpy arrays
color_image = np.asanyarray(color_frame.get_data())
# # Stack both images horizontally
# images = np.hstack(color_image)
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', color_image)
if flag_cap:
# # Take samle
# 1. Image
cnt = cnt + 1
cv2.imwrite('Img_marker' + str(cnt) + '.jpg', color_image)
# 2. End effector Pose
ee_pose = left_arm.endpoint_pose()
ee_position = ee_pose['position']
ee_orientation = ee_pose['orientation']
quat = [
ee_orientation.x, ee_orientation.y, ee_orientation.z,
ee_orientation.w
]
T_matrix = quaternion_matrix(
quat) # order need to be checked, here is xyzw
transl = np.array([ee_position.x, ee_position.y, ee_position.z])
T_matrix[0:3, 3] = transl.T
print('id', cnt, 'EE_pose', T_matrix)
T_matrix = np.array(T_matrix)
f_name = str('EE_pose' + str(cnt) + '.txt')
np.savetxt(f_name, T_matrix, fmt='%.6f', delimiter=',')
# fo = open('EE_pose' + str(cnt) + '.txt', 'w')
# fo.close
flag_cap = 0 # reset flag
key = cv2.waitKey(50)
if key & 0xFF == ord('t'):
flag_cap = 1 # set flag
elif key & 0xFF == ord('q'):
break
tracker = track(pr, pl_init)
tracker.busy = True
done = False
ik()
#print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
tracker.track()
if tracker.busy == False:
tracker.gripper.close()
rospy.sleep(0.5)
arm = Limb("right")
ep_position = arm.endpoint_pose()['position']
ep_orientation = arm.endpoint_pose()['orientation']
place(right, ep_position, ep_orientation, pr, pr_place)
tracker.gripper.open()
rospy.sleep(0.5)
pr_place.position.y += 0.2
move(right, pr_place)
#move(right,pr_init)
print "I am done"
done = True
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
print("Program finished")
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
class IKHelper(object):
"""An abstraction layer for using Baxter's built in IK service."""
############################################################################
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(_IKSVC_LEFT_URI, SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(250)
############################################################################
def reset(self):
"""Reset both arms to their neutral positions."""
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def set_left(self, pos, rot=(0, math.pi, math.pi * 0.5), wait=False):
"""Set the endpoint of the left arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait)
def set_left(self, pos, rot=(0, math.pi, math.pi * 0.5), wait=False):
"""Set the endpoint of the right arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait)
def get_left(self):
"""Return the current endpoint pose of the left arm."""
return self._left_arm.endpoint_pose()['position']
def get_right(self):
"""Return the current endpoint pose of the left arm."""
return self._right_arm.endpoint_pose()['position']
def get_left_velocity(self):
"""Return the current endpoint velocity of the left arm."""
return self._left_arm.endpoint_velocity()['linear']
def get_right_velocity(self):
"""Return the current endpoint velocity of the right arm."""
return self._right_arm.endpoint_velocity()['linear']
def get_left_force(self):
"""Return the current endpoint force on the left arm."""
return self._left_arm.endpoint_effort()['force']
def get_right_force(self):
"""Return the current endpoint force on the right arm."""
return self._right_arm.endpoint_effort()['force']
############################################################################
def _set_arm(self, iksvc, limb, pos, rot, wait):
resp = self._get_ik(iksvc, pos, rot)
positions = resp[0]
isValid = resp[1]
if not isValid:
print('invalid: {0} {1} {2}'.format(x, y, z))
if not wait:
limb.set_joint_positions(positions)
else:
limb.move_to_joint_positions(positions)
def _get_ik(self, iksvc, pos, rot):
q = quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
iksvc.wait_for_service(5.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
class IKHelper(object):
"""An abstraction layer for using Baxter's built in IK service."""
############################################################################
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(
_IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16)
self._joint_update_pub.publish(250)
############################################################################
def reset(self):
"""Reset both arms to their neutral positions."""
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
"""Set the endpoint of the left arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait)
def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
"""Set the endpoint of the right arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait)
def get_left(self):
"""Return the current endpoint pose of the left arm."""
return self._left_arm.endpoint_pose()['position']
def get_right(self):
"""Return the current endpoint pose of the left arm."""
return self._right_arm.endpoint_pose()['position']
def get_left_velocity(self):
"""Return the current endpoint velocity of the left arm."""
return self._left_arm.endpoint_velocity()['linear']
def get_right_velocity(self):
"""Return the current endpoint velocity of the right arm."""
return self._right_arm.endpoint_velocity()['linear']
def get_left_force(self):
"""Return the current endpoint force on the left arm."""
return self._left_arm.endpoint_effort()['force']
def get_right_force(self):
"""Return the current endpoint force on the right arm."""
return self._right_arm.endpoint_effort()['force']
############################################################################
def _set_arm(self, iksvc, limb, pos, rot, wait):
resp = self._get_ik(iksvc, pos, rot)
positions = resp[0]
isValid = resp[1]
if not isValid:
print('invalid: {0} {1} {2}'.format(x, y, z))
if not wait:
limb.set_joint_positions(positions)
else:
limb.move_to_joint_positions(positions)
def _get_ik(self, iksvc, pos, rot):
q = quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
iksvc.wait_for_service(5.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
