class Robot_Gripper(object):
def __init__(self):
self._gripper = Gripper('left')
self._gripper.calibrate()
#right gripper isn't calibrated so it won't drop the RFID reader
#self._gripper_right = Gripper('right')
#self._gripper_right.calibrate()
def close(self, left=True):
if left:
self._gripper.close(block=True)
else:
self._gripper_right.close(block=True)
def open(self, left=True):
if left:
self._gripper.open(block=True)
else:
self._gripper_right.open(block=True)
def gripping(self, left=True):
if left:
return self._gripper.gripping()
else:
return self._gripper_right.gripping()
def main():
global thetas
configureLeftHand()
planner = PathPlanner("right_arm")
grip = Gripper('right')
grip.calibrate()
raw_input("gripper calibrated")
grip.open()
table_size = np.array([3, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -5 - .112
#put cup
cup_pos = start_pos_xy
end_pos = goal_pos_xy
putCupObstacle(planner, cup_pos, "cup1")
putCupObstacle(planner, end_pos, "cup2")
planner.add_box_obstacle(table_size, "table", table_pose)
#move gripper to behind cup
position = cup_pos + np.array([-0.1, 0, -0.02])
orientation = np.array([0, np.cos(np.pi / 4), 0, np.cos(np.pi / 4)])
executePositions(planner, [position], [orientation])
raw_input("moved behind cup, press enter")
#move to cup and remove cup1 as obstacle since picking up
removeCup(planner, "cup1")
position = cup_pos + np.array([0, 0, -0.05])
executePositions(planner, [position], [orientation])
raw_input("moved to cup, press to close")
grip.close()
rospy.sleep(1)
raw_input("gripped")
moveAndPour(planner, end_pos)
raw_input("poured")
executePositions(planner, [position + np.array([0, 0, 0.02])],
[orientation])
grip.open()
removeCup(planner, "cup2")
class Abbe_Gripper(object):
def __init__(self):
self._gripper = Gripper('left')
self._gripper.calibrate()
#right gripper isn't calibrated so it won't drop the RFID reader
#self._gripper_right = Gripper('right')
#self._gripper_right.calibrate()
def close(self,left=True):
if left:
self._gripper.close(block=True)
else:
self._gripper_right.close(block=True)
def open(self,left=True):
if left:
self._gripper.open(block=True)
else:
self._gripper_right.open(block=True)
def gripping(self,left=True):
if left:
return self._gripper.gripping()
else:
return self._gripper_right.gripping()
class Abbe_Gripper(object):
def __init__(self):
self._gripper = Gripper('left')
self._gripper.calibrate()
#don't want it to drop rfid reader so disabling right gripperq
self._gripper_right = Gripper('right')
self._gripper_right.calibrate()
def close(self,left=True):
if left:
self._gripper.close(block=True)
else:
self._gripper_right.close(block=True)
def open(self,left=True):
if left:
self._gripper.open(block=True)
else:
self._gripper_right.open(block=True)
def gripping(self,left=True):
if left:
return self._gripper.gripping()
else:
return self._gripper_right.gripping()
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 MoveItArm:
"""
Represents Baxter's Arm in MoveIt! world.
Represents a hand of Baxter through MoveIt! Group. Some reusable code has
been packed together into this class to make the code more readable.
"""
def __init__(self, side_name):
"""Will configure and initialise Baxter's hand."""
self._side_name = side_name
# This is the reference to Baxter's arm.
self.limb = MoveGroupCommander("{}_arm".format(side_name))
self.limb.set_end_effector_link("{}_gripper".format(side_name))
# Unfortunetly, MoveIt was not able to make the gripper work, neither
# did was able to find a very good pose using Forward Kinematics,
# so instead the Baxter SDK is used here to do these two jobs.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self._limb = Limb(side_name)
# This solver seems to be better for finding solution among obstacles
self.limb.set_planner_id("RRTConnectkConfigDefault")
# Error tollerance should be as low as possible for better accuracy.
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
def __str__(self):
"""
String representation of the arm.
String representation of the arm, either 'left' or 'right' string will
be returned. Useful when the string representation of the arm is
needed, like when accessing the IK solver.
"""
return self._side_name
def is_left(self):
"""Will return True if this is the left arm, false otherwise."""
return True if self._side_name == "left" else False
def is_right(self):
"""Will return True if this is the right arm, false otherwise."""
return True if self._side_name == "right" else False
def open_gripper(self):
"""Will open Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.open(block=True)
def close_gripper(self):
"""Will close Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.close(block=True)
class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the DigitalIO Signal feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control {left, right}
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._close_io = DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn
self._open_io = DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn
self._light_io = DigitalIO('%s_lower_cuff' % (arm,)) # cuff squeeze
# outputs
self._gripper = Gripper('%s' % (arm,))
self._nav = Navigator('%s' % (arm,))
# connect callback fns to signals
if self._gripper.type() != 'custom':
self._gripper.calibrate()
self._open_io.state_changed.connect(self._open_action)
self._close_io.state_changed.connect(self._close_action)
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
if lights:
self._light_io.state_changed.connect(self._light_action)
rospy.loginfo("%s Cuff Control initialized...",
self._gripper.name.capitalize())
def _open_action(self, value):
if value:
rospy.logdebug("gripper open triggered")
self._gripper.open()
def _close_action(self, value):
if value:
rospy.logdebug("gripper close triggered")
self._gripper.close()
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
self._nav.inner_led = value
self._nav.outer_led = value
class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the DigitalIO Signal feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control {left, right}
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._close_io = DigitalIO('%s_upper_button' % (arm, )) # 'dash' btn
self._open_io = DigitalIO('%s_lower_button' % (arm, )) # 'circle' btn
self._light_io = DigitalIO('%s_lower_cuff' % (arm, )) # cuff squeeze
# outputs
self._gripper = Gripper('%s' % (arm, ))
self._nav = Navigator('%s' % (arm, ))
# connect callback fns to signals
if self._gripper.type() != 'custom':
self._gripper.calibrate()
self._open_io.state_changed.connect(self._open_action)
self._close_io.state_changed.connect(self._close_action)
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
if lights:
self._light_io.state_changed.connect(self._light_action)
rospy.loginfo("%s Cuff Control initialized...",
self._gripper.name.capitalize())
def _open_action(self, value):
if value:
rospy.logdebug("gripper open triggered")
self._gripper.open()
def _close_action(self, value):
if value:
rospy.logdebug("gripper close triggered")
self._gripper.close()
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
self._nav.inner_led = value
self._nav.outer_led = value
def main():
factor=9
print("Right Init node")
rospy.init_node("Right_Ik_service_client", anonymous=True)
rate= rospy.Rate(1000)
right_gripper=Gripper('right')
right_gripper.calibrate()
right_gripper.open()
while not rospy.is_shutdown():
initial_position(round(6000*factor))#10ms
go_position(round(10000*factor),0.25)
for x in range (0,int(round(2000*factor))):
right_gripper.close()
#initial_position(10000)#3ms
go_box(round(250*factor))
for x in range (0,int(round(800*factor))):
right_gripper.open()
print "Objecto dejado"
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)
#!/usr/bin/env python
import rospy
from baxter_interface import Gripper
if __name__ == '__main__':
rospy.init_node('gripperTools', anonymous=True)
gripper = Gripper('right')
gripper.close(True)
class GraspExecuter(object):
def __init__(self, max_vscale=1.0):
self.grasp_queue = Queue()
initialized = False
while not initialized:
try:
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
self.left_arm.set_max_velocity_scaling_factor(max_vscale)
self.go_to_pose('left', L_HOME_STATE)
self.right_arm = moveit_commander.MoveGroupCommander(
'right_arm')
self.right_arm.set_max_velocity_scaling_factor(max_vscale)
self.go_to_pose('right', R_HOME_STATE)
self.left_gripper = Gripper('left')
self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE)
self.left_gripper.open()
self.right_gripper = Gripper('right')
self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE)
self.right_gripper.open()
initialized = True
except rospy.ServiceException as e:
rospy.logerr(e)
def queue_grasp(self, req):
grasp = req.grasp
rotation_quaternion = np.asarray([
grasp.pose.orientation.w, grasp.pose.orientation.x,
grasp.pose.orientation.y, grasp.pose.orientation.z
])
translation = np.asarray([
grasp.pose.position.x, grasp.pose.position.y, grasp.pose.position.z
])
T_grasp_camera = RigidTransform(rotation_quaternion, translation,
'grasp', T_camera_world.from_frame)
T_gripper_world = T_camera_world * T_grasp_camera * T_gripper_grasp
self.grasp_queue.put(T_gripper_world.pose_msg)
return True
def execute_grasp(self, grasp):
if grasp.position.z < TABLE_DEPTH + 0.02:
grasp.position.z = TABLE_DEPTH + 0.02
approach = deepcopy(grasp)
approach.position.z = grasp.position.z + GRASP_APPROACH_DIST
# perform grasp on the robot, up until the point of lifting
rospy.loginfo('Approaching')
self.go_to_pose('left', approach)
# grasp
rospy.loginfo('Grasping')
self.go_to_pose('left', grasp)
self.left_gripper.close(block=True)
#lift object
rospy.loginfo('Lifting')
self.go_to_pose('left', approach)
# Drop in bin
rospy.loginfo('Going Home')
self.go_to_pose('left', L_PREGRASP_POSE)
self.left_gripper.open()
lift_gripper_width = self.left_gripper.position() # a percentage
# check drops
lifted_object = lift_gripper_width > 0.0
return lifted_object, lift_gripper_width
def go_to_pose(self, arm, pose):
"""Uses Moveit to go to the pose specified
Parameters
----------
pose : :obj:`geometry_msgs.msg.Pose` or RigidTransform
The pose to move to
max_velocity : fraction of max possible velocity
"""
if arm == 'left':
arm = self.left_arm
else:
arm = self.right_arm
arm.set_start_state_to_current_state()
arm.set_pose_target(pose)
arm.plan()
arm.go()
class Loop:
def __init__(self):
abs_path = "/home/hakan/ros_ws/src/baxter_examples/scripts/ASM/"
with open(abs_path + 'models/dmp_model.pkl', 'rb') as input:
[self.dmp_data, self.DMPObject] = pickle.load(input)
# Uncomment this if force data available.
with open(abs_path + "models/force_model.pkl", 'rb') as input:
self.HMMObject = pickle.load(input)
self.Kx = 0
self.Ky = 80
self.alpha = 0.98
self.slow = 1.0
self.limb_side = 'right'
# Start ROS
self.limb = Limb(self.limb_side)
self.kin = baxter_kinematics(self.limb_side)
self.gripper = Gripper(self.limb_side)
self.gripper.calibrate()
self.gripper.close()
# DMP Parameters initialization
self.dt = 1.0 / 100 # denominator is recording rate of this file
self.current_end_pose = copy.deepcopy(self.dmp_data.end_pose[0, :])
self.initial_end_pose = copy.deepcopy(self.current_end_pose[:])
self.goal_end_pose = copy.deepcopy(self.dmp_data.end_pose[-1, :])
self.desired_end_pose = copy.deepcopy(self.current_end_pose[:])
self.des_tau = copy.deepcopy(self.dmp_data.time[-1])
self.current_end_velocities = np.zeros(7)
self.desired_end_velocities = copy.deepcopy(
self.current_end_velocities[:])
self.zeta = np.zeros(7)
# Going to initial position by Inverse Kinematics
# self.desired_end_pose = poseEuler2pose(self.desired_end_pose)
# self.desired_joint_positions = self.inverse_kinematics(self.desired_end_pose)
self.desired_joint_positions = copy.deepcopy(
self.dmp_data.joint_positions[0, :])
self.current_joint_positions = copy.deepcopy(
self.desired_joint_positions[:])
self.limb.move_to_joint_positions(
array2dict(self.desired_joint_positions, self.limb_side))
# Variables to feed the other functions
self.t = 0 # to feed GMR
self.step = 0 # to feed savgol_filter
# Plotting variables holders initialization
self.current_joint_position_holder = [self.desired_joint_positions]
self.desired_joint_position_holder = [self.desired_joint_positions]
self.current_end_pose_holder = [get_end_pose(self.limb)]
self.desired_end_pose_holder = [get_end_pose(self.limb)]
self.time_holder = []
self.current_forces_holder = []
self.desired_forces_holder = []
self.error_prev = 0
def inverse_kinematics(self, desired_end_pose):
current_joint_positions = get_joint_positions(self.limb,
self.limb_side).tolist()
desired_joint_positions = self.kin.inverse_kinematics(desired_end_pose[:3].tolist(),\
desired_end_pose[3:].tolist())
if desired_joint_positions == None:
return current_joint_positions
else:
return desired_joint_positions
def compute_zeta(self):
self.desired_forces_holder.append(
gmr(self.t, self.HMMObject.mu, self.HMMObject.sigma).reshape(
(6, )))
error_now = (
get_wrench(self.limb) -
gmr(self.t, self.HMMObject.mu, self.HMMObject.sigma).reshape(
(6, )))
error = (1 - self.alpha) * error_now + self.alpha * self.error_prev
self.error_prev = error
self.zeta[0] = self.Kx * error[0]
self.zeta[1] = self.Ky * error[1]
def loop(self):
# Real-time part
while rospy.get_time() == 0:
pass
rate = rospy.Rate(1 / self.dt)
# while not rospy.is_shutdown() and self.t <= self.des_tau:
while not rospy.is_shutdown() and self.t <= 9.45:
self.time_holder.append(self.t)
self.current_forces_holder.append(get_wrench(self.limb))
self.compute_zeta() # uncomment this if force data available.
self.desired_end_pose, self.desired_end_velocities = \
self.DMPObject.dmp.execute(self.t, self.dt/self.slow, self.des_tau,
self.initial_end_pose,
self.goal_end_pose,
self.desired_end_pose,
self.desired_end_velocities,
zeta=self.zeta)
desired_end_pose = np.append(self.desired_end_pose[:3],
self.dmp_data.end_pose[self.step, 3:])
self.desired_joint_positions = self.inverse_kinematics(
desired_end_pose)
self.limb.set_joint_positions(
array2dict(self.desired_joint_positions, self.limb_side))
self.t += self.dt / self.slow
self.step += 1
self.current_joint_position_holder.append(
get_joint_positions(self.limb, self.limb_side))
self.desired_joint_position_holder.append(
self.desired_joint_positions)
self.current_end_pose_holder.append(get_end_pose(self.limb))
self.desired_end_pose_holder.append(self.desired_end_pose)
rate.sleep()
class RobotArm:
"""
A class used to represent the arm of a robot
using both "Moveit" API and Baxter SDK API.
"""
def __init__(self, side_name):
"""
Constructor.
@param side_name: which arm side we are refering to 'left' or 'right'.
"""
self.__side_name = side_name
# MoveIt interface to a group of arm joints.
# Either left arm joint group or right arm joint group.
self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name))
# MoveIt limb setting.
self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name))
self.moveit_limb.set_planner_id('RRTConnectKConfigDefault')
self.moveit_limb.set_goal_position_tolerance(0.01)
self.moveit_limb.set_goal_orientation_tolerance(0.01)
# MoveIt does not provide support for Baxter gripper.
# Thus we use Baxter SDK gripper instead.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self.baxter_sdk_limb = Limb(side_name)
def __str__(self):
"""
Built-in function to return a printable string
representing the arm used in either print(object)
or str(object).
"""
return str(self.__side_name)
def open_gripper(self):
"""
Command the robot arm end-effector i.e. the gripper to open.
"""
self.gripper.open(block=True)
def close_gripper(self):
"""
Command the robot arm end-effector i.e. the gripper to close.
"""
self.gripper.close(block=True)
def is_left_arm(self):
"""
Return true if the arm is corresponding to the left arm.
"""
return self.__side_name == 'left'
def is_right_arm(self):
"""
Return true if the arm is corresponding to the right arm.
"""
return self.__side_name == 'right'
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}
)
#lowers to grab cookie
left.move_to_joint_positions({'left_w0': -1.2367720102326147, 'left_w1': 0.5027622032294443, 'left_w2': 3.008519820240268, 'left_e0': 0.33709227813781967, 'left_e1': 1.2870098810358621, 'left_s0': -0.5944175553055978, 'left_s1': -0.3739078170470696}
)
#swoops in
left.move_to_joint_positions({'left_w0': -1.4741555371578825, 'left_w1': 0.4049709280017492, 'left_w2': 3.0158062289827234, 'left_e0': 0.3324903357741634, 'left_e1': 1.2421409429902137, 'left_s0': -0.6270146470481629, 'left_s1': -0.27228158984966094}
)
#grippers close
leftg.close()
#lifts a little bit
left.move_to_joint_positions({'left_w0': -1.474539032354854, 'left_w1': 0.3278883934105072, 'left_w2': 3.0165732193766663, 'left_e0': 0.33325732616810616, 'left_e1': 1.2950632801722606, 'left_s0': -0.6645971763513555, 'left_s1': -0.362402961137929}
)
#lifts more
left.move_to_joint_positions({'left_w0': -1.476840003536682, 'left_w1': 0.26307770512234846, 'left_w2': 3.0108207914220957, 'left_e0': 0.35856800916821546, 'left_e1': 1.4848934026730805, 'left_s0': -0.8279661302611521, 'left_s1': -0.7827136970185323}
)
#head turns to customer
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
#lowers
left.move_to_joint_positions({'left_w0': -1.5079031144913617, 'left_w1': 0.2067039111675595, 'left_w2': 3.008519820240268, 'left_e0': 0.3466796580621035, 'left_e1': 0.8847234194129123, 'left_s0': -0.9955535313376335, 'left_s1': -0.11543205428837738}
)
'left_s1': -0.3739078170470696
})
#swoops in
left.move_to_joint_positions({
'left_w0': -1.4741555371578825,
'left_w1': 0.4049709280017492,
'left_w2': 3.0158062289827234,
'left_e0': 0.3324903357741634,
'left_e1': 1.2421409429902137,
'left_s0': -0.6270146470481629,
'left_s1': -0.27228158984966094
})
#grippers close
leftg.close()
#lifts a little bit
left.move_to_joint_positions({
'left_w0': -1.474539032354854,
'left_w1': 0.3278883934105072,
'left_w2': 3.0165732193766663,
'left_e0': 0.33325732616810616,
'left_e1': 1.2950632801722606,
'left_s0': -0.6645971763513555,
'left_s1': -0.362402961137929
})
#lifts more
left.move_to_joint_positions({
'left_w0': -1.476840003536682,
class Baxterpicknumber():
def __init__(self):
rospy.loginfo("++++++++++++Ready to move the robot+++++++++++")
#Initialize moveit_commander
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
#Display Trajectory in RVIZ
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.moving_publisher = rospy.Publisher('amimoving',
String,
queue_size=10)
rospy.sleep(3.0)
# print self.robot.get_current_state()
# print ""
self.planning_frame = self.right_arm_group.get_planning_frame()
rospy.loginfo("============ Reference frame: %s" % self.planning_frame)
self.eef_link = self.right_arm_group.get_end_effector_link()
print "============ End effector: %s" % self.eef_link
self.group_names = self.robot.get_group_names()
print "============ Robot Groups:", self.robot.get_group_names()
#set planning and execution parameters
self.right_arm_group.set_goal_position_tolerance(0.01)
self.right_arm_group.set_goal_orientation_tolerance(0.01)
self.right_arm_group.set_planning_time(5.0)
self.right_arm_group.allow_replanning(False)
self.right_arm_group.set_max_velocity_scaling_factor(0.6)
self.right_arm_group.set_max_acceleration_scaling_factor(0.6)
#specify which gripper and limb are taking command
self.right_gripper = Gripper('right', CHECK_VERSION)
self.right_gripper.reboot()
self.right_gripper.calibrate()
# self.limb = baxter_interface.Limb('right')
# self.angles = self.limb.joint_angles()
# self.wave_1 = {'right_s0': -0.459, 'right_s1': -0.202, 'right_e0': 1.807, 'right_e1': 1.714, 'right_w0': -0.906, 'right_w1': -1.545, 'right_w2': -0.276}
# rospy.sleep(3)
# self.limb.move_to_joint_positions(self.wave_1)
self.pose_target = Pose()
self.standoff = 0.1
self.__right_sensor = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range, self.rangecallback)
self.rangestatetemp = Range()
self.box_name = 'table'
self.moving = '0'
self.moving_publisher.publish(self.moving)
def add_box(self, timeout=4):
self.box_pose = PoseStamped()
self.box_pose.header.frame_id = "base"
self.box_pose.pose.position.x = 1.0
self.box_pose.pose.position.z = -0.27
self.box_pose.pose.orientation.w = 1.0
self.scene.add_box(self.box_name, self.box_pose, size=(1.5, 2, 0.1))
# return self.wait_for_state_update(box_is_known=True, timeout=timeout)
def wait_for_state_update(self,
box_is_known=False,
box_is_attached=False,
timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
## BEGIN_SUB_TUTORIAL wait_for_scene_update
##
## Ensuring Collision Updates Are Receieved
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## If the Python node dies before publishing a collision object update message, the message
## could get lost and the box will not appear. To ensure that the updates are
## made, we wait until we see the changes reflected in the
## ``get_known_object_names()`` and ``get_known_object_names()`` lists.
## For the purpose of this tutorial, we call this function after adding,
## removing, attaching or detaching an object in the planning scene. We then wait
## until the updates have been made or ``timeout`` seconds have passed
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# Test if the box is in attached objects
attached_objects = scene.get_attached_objects([box_name])
is_attached = len(attached_objects.keys()) > 0
# Test if the box is in the scene.
# Note that attaching the box will remove it from known_objects
is_known = box_name in scene.get_known_object_names()
# Test if we are in the expected state
if (box_is_attached == is_attached) and (box_is_known == is_known):
return True
# Sleep so that we give other threads time on the processor
rospy.sleep(0.1)
seconds = rospy.get_time()
# If we exited the while loop without returning then we timed out
return False
def rangecallback(self, data):
self.rangestatetemp = data
self.rangestate = self.rangestatetemp.range
def compute_cartesian(self): #,data):
testingdata = np.array([0.6, -0.320147457674, -0.209, np.pi])
data = testingdata
rospy.loginfo("++++++++++Received desired position+++++++++")
#extract x,y,z coordinate from subscriber data
self.x_coord = data[0]
self.y_coord = data[1]
self.z_coord = data[2]
theta = data[3]
#put coordinate data back to arrays
pout = np.array([self.x_coord, self.y_coord, self.z_coord])
block_orientation = tr.quaternion_from_euler(theta, 0, 0, 'sxyz')
#convert orentation and position to Quaternion
quat = Quaternion(*block_orientation)
self.quat_position = Point(*pout)
self.quat_orientation = copy.deepcopy(quat)
self.cartesian_reader = True
# def move_arm_standoff_cartesianpath(self):
# self.moving=True
# waypoints=[]
# rospy.sleep(1.0)
# wpose=self.pose_target
# wpose.posistion.z= self.pose_target
# wpose.position=self.quat_position
# waypoints.append(copy.deepcopy(wpose))
# print('moving to standoff')
# (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.01,0.0)
# self.right_arm_group.execute(plan,wait=True)
def move_arm_standoff(self):
self.moving = '1'
self.moving_publisher.publish(self.moving)
rospy.loginfo("+++++++++++++Going to standoff posistion+++++++++")
#def standoff height (above the block)
#standoff position has same gripper orentation as block orientation
self.pose_target.orientation = self.quat_orientation
#add standoff height to the height of the block and get new pose_target.position
z_standoff = self.z_coord + self.standoff
pout = np.array([self.x_coord, self.y_coord, z_standoff])
self.pose_target.position = Point(*pout)
#Calling Moveit to use IK to compute the plan and execute it
print(self.pose_target)
self.right_arm_group.set_pose_target(self.pose_target)
rospy.sleep(3.0)
plan_standoff = self.right_arm_group.go(wait=True)
self.right_arm_group.stop()
self.right_arm_group.clear_pose_targets()
rospy.sleep(1.0)
# def move_arm_pick_cartesianpath(self,scale=1):
# waypoints=[]
# rospy.sleep(1.0)
# wpose=self.pose_target
# wpose.position=self.quat_position
# waypoints.append(copy.deepcopy(wpose))
# print('moving to pick up')
# (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.01,0.0)
# self.right_arm_group.execute(plan,wait=True)
def remove_box(self):
self.scene.remove_world_object(self.box_name)
def move_arm_pick(self):
rospy.loginfo("+++++++++++++Going to pickup posistion+++++++++")
#standoff position has same gripper orentation and height as block orientation
self.pose_target.orientation = self.quat_orientation
z_standoff = self.z_coord
pout = np.array([self.x_coord, self.y_coord, z_standoff])
self.pose_target.position = Point(*pout)
#Calling Moveit to use IK to compute the plan and execute it
print(self.pose_target)
self.right_arm_group.set_pose_target(self.pose_target)
rospy.sleep(2.0)
plan_standoff = self.right_arm_group.go(wait=True)
print(self.rangestate)
if self.rangestate < 0.4:
self.right_gripper.close()
else:
self.right_gripper.open()
self.right_arm_group.stop()
self.right_arm_group.clear_pose_targets()
rospy.sleep(1.0)
# def move_arm_back_standoff_cartesianpath(self,scale=1):
# waypoints=[]
# rospy.sleep(1.0)
# wpose=self.pose_target
# waypoints.append(copy.deepcopy(wpose))
# (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.005,0.01)
# self.right_arm_group.execute(plan,wait=True)
def move_arm_back_standoff(self):
rospy.loginfo(
"+++++++++++++Going back to stand off posistion+++++++++")
self.right_gripper.close()
#def standoff height (above the block)
#standoff position has same gripper orentation as block orientation
self.pose_target.orientation = self.quat_orientation
#add standoff height to the height of the block and get new pose_target.position
z_standoff = self.z_coord + self.standoff
pout = np.array([self.x_coord, self.y_coord, z_standoff])
self.pose_target.position = Point(*pout)
#Calling Moveit to use IK to compute the plan and execute it
print(self.pose_target)
self.right_arm_group.set_pose_target(self.pose_target)
rospy.sleep(2.0)
plan_backtostandoff = self.right_arm_group.go(wait=True)
self.right_arm_group.stop()
self.right_arm_group.clear_pose_targets()
rospy.sleep(1.0)
# def move_arm_standoff2_cartesianpath(self,scale=1):
# self.pose_target.position=self.quat_position
# waypoints=[]
# rospy.sleep(1.0)
# wpose=self.pose_target
# wpose.position.z = scale * self.standoff
# waypoints.append(copy.deepcopy(wpose))
# (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.01,0.0)
# self.right_arm_group.execute(plan,wait=True)
def move_arm_standoff2(self):
rospy.loginfo("+++++++++++++Going to standoff2 posistion+++++++++")
#def standoff height (above the block)
self.right_gripper.close()
#standoff position has same gripper orentation as block orientation
self.pose_target.orientation = self.quat_orientation
#add standoff height to the height of the block and get new pose_target.position
x_standoff2 = self.x_coord
z_standoff2 = self.z_coord + self.standoff
y_standoff2 = self.y_coord + 0.2
pout = np.array([x_standoff2, y_standoff2, z_standoff2])
self.pose_target.position = Point(*pout)
#Calling Moveit to use IK to compute the plan and execute it
print(self.pose_target)
self.right_arm_group.set_pose_target(self.pose_target)
rospy.sleep(3.0)
plan_standoff2 = self.right_arm_group.go(wait=True)
self.right_arm_group.stop()
self.right_arm_group.clear_pose_targets()
rospy.sleep(1.0)
# def move_arm_place_cartesianpath(self,scale=1):
# waypoints=[]
# rospy.sleep(1.0)
# wpose=self.pose_target
# wpose.position.z = self.quat_position.z
# waypoints.append(copy.deepcopy(wpose))
# (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.005,0.01)
# self.right_arm_group.execute(plan,wait=True)
def move_arm_place(self):
global placedtimes
rospy.loginfo("+++++++++++++Going to place posistion+++++++++")
self.right_gripper.close()
#standoff position has same gripper orentation and height as block orientation
self.pose_target.orientation = self.quat_orientation
# z_standoff=block_height/2 in case need gripper half height higer than predicted block height.
self.pose_target.position = self.quat_position
self.placetarget_y = np.linspace(0.2, 0.29, 10)
x_place = self.x_coord
z_place = self.z_coord
y_place = self.y_coord + self.placetarget_y[placedtimes]
pout = np.array([x_place, y_place, z_place])
self.pose_target.position = Point(*pout)
#Calling Moveit to use IK to compute the plan and execute it
print(self.pose_target)
self.right_arm_group.set_pose_target(self.pose_target)
rospy.sleep(2.0)
plan_place = self.right_arm_group.go(wait=True)
self.right_gripper.open()
self.right_arm_group.stop()
self.right_arm_group.clear_pose_targets()
rospy.sleep(1.0)
def move_arm_backstandoff2(self):
global placedtimes
rospy.loginfo(
"+++++++++++++Going back to standoff2 posistion+++++++++")
#def standoff height (above the block)
self.right_gripper.open()
#standoff position has same gripper orentation as block orientation
self.pose_target.orientation = self.quat_orientation
#add standoff height to the height of the block and get new pose_target.position
x_standoff2 = self.x_coord
z_standoff2 = self.z_coord + self.standoff
y_standoff2 = self.y_coord + self.placetarget_y[placedtimes]
pout = np.array([x_standoff2, y_standoff2, z_standoff2])
self.pose_target.position = Point(*pout)
#Calling Moveit to use IK to compute the plan and execute it
print(self.pose_target)
self.right_arm_group.set_pose_target(self.pose_target)
rospy.sleep(2.0)
plan_backstandoff2 = self.right_arm_group.go(wait=True)
self.right_arm_group.stop()
self.right_arm_group.clear_pose_targets()
rospy.sleep(1.0)
placedtimes += 1
self.moving = '0'
self.moving_publisher.publish(self.moving)
class SuctionPNPTask:
"""
A representation of a generic pick-and-place task using a suction cup.
Attributes:
gripper_side (str): 'left' or 'right'.
planner: The path planner.
gripper: The gripper.
vacuum_sensor: The vacuum sensor.
"""
GRIP_MAX_VALUE = 175
def __init__(self, frame_id='base', gripper_side='right'):
assert gripper_side in ('left', 'right')
self.gripper_side = gripper_side
self.camera_side = 'left' if self.gripper_side == 'right' else 'right'
self.gripper_planner = PathPlanner(frame_id,
self.gripper_side + '_arm')
self.camera_planner = PathPlanner(frame_id, self.camera_side + '_arm')
self.calibrate_gripper()
if rospy.get_param('verbose'):
rospy.loginfo('Initialized PNP task.')
def set_joint_angles(self,
angles,
limb,
tolerance=0.05,
max_steps=1000,
period=0.01):
all_angles = dict(angles)
for name in limb.joint_names():
if name not in all_angles:
all_angles[name] = limb.join_angle(name)
step = 0
done = lambda: all(
abs(angle - limb.joint_angle(name)) < tolerance
for name, angle in all_angles.items())
while step < max_steps and not done():
limb.set_joint_positions(all_angles)
rospy.sleep(period)
step += 1
return done()
def is_grasping(self, threshold=80):
"""
Uses the vacuum sensor to detect whether the suction cup has grasped an
object. The topics
/robot/analog_io/left_vacuum_sensor_analog/value_uint32
/robot/analog_io/left_vacuum_sensor_analog/state
both give the direct analog readings from the vacuum sensor in the
gripper. According to our gripper engineer, the values mean:
0 - 46: The vacuum gripper is likely not attached to an object.
47 - 175: The vacuum is likely attached to an object (usually
around 150 when grasping).
176+: There is likely a short between 5V and signal on the sensor.
Arguments:
threshold (int): Values above this threshold constitute a grip.
"""
gripper_value = self.vacuum_sensor.state()
if rospy.get_param('verbose'):
rospy.loginfo('Current vacuum value: {}'.format(gripper_value))
if gripper_value > self.GRIP_MAX_VALUE:
raise ValueError(
'Detected unsafe vacuum value of {}.'.format(gripper_value))
return gripper_value > threshold
def calibrate_gripper(self):
""" Initialize the gripper and its vacuum sensor. """
self.gripper = Gripper(self.gripper_side)
self.gripper.calibrate()
self.gripper.open()
self.vacuum_sensor = AnalogIO(self.gripper_side +
'_vacuum_sensor_analog')
if rospy.get_param('verbose'):
rospy.loginfo('Calibrated gripper. (type={})'.format(
self.gripper.type()))
def open_gripper(self, delay=1):
""" Open the gripper with a given delay afterwards. """
self.gripper.open()
rospy.sleep(delay)
if rospy.get_param('verbose'):
rospy.loginfo('Opened gripper.')
def close_gripper(self, delay=1):
""" Close a gripper with a given delay afterwards. """
self.gripper.close()
rospy.sleep(delay)
if rospy.get_param('verbose'):
rospy.loginfo('Closed gripper.')
def __init__(self):
pygame.init()
rospy.init_node('node_mouse', anonymous=True)
right_gripper=Gripper('right')
left_gripper=Gripper('left')
#limb_0=baxter_interface.Limb('right')
#limb_0.set_joint_positions({'right_w2': 0.00})
def set_w2():
limb=baxter_interface.Limb('left')
current_position = limb.joint_angle('left_w2')
#send=current_position+delta
#if(send>2.80 or send<-2.80):
# delta=-delta
# time.sleep(0.15)
#print send
return current_position
#joint_command = {joint_name: send}
#limb.set_joint_positions(joint_command)
#if(current_position-send>0):
# delta=-1
def set_s1():
limb=baxter_interface.Limb('left')
current_position = limb.joint_angle('left_s1')
#send=current_position+delta
#if(send>2.80 or send<-2.80):
# delta=-delta
# time.sleep(0.15)
#print send
return current_position
def set_e1():
limb=baxter_interface.Limb('left')
current_position = limb.joint_angle('left_e1')
#send=current_position+delta
#if(send>2.80 or send<-2.80):
# delta=-delta
# time.sleep(0.15)
#print send
return current_position
pygame.display.set_caption('Game')
pygame.mouse.set_visible(True)
vec = Vector3(1, 2, 3)
right_gripper.calibrate()
left_gripper.calibrate()
try:
pub = rospy.Publisher('mouse', Vector3, queue_size=1)
rate = rospy.Rate(1000) # 10hz
screen = pygame.display.set_mode((640,480), 0, 32)
pygame.mixer.init()
a=0 ,0 ,0
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEBUTTONUP:
a = pygame.mouse.get_pressed()
print a
weq=0
if a[0]==1:
left_gripper.open()
else:
left_gripper.close()
if a[2]==1:
right_gripper.open()
else:
right_gripper.close()
vec.x=set_w2()
vec.y=a[1]
vec.z=0
print vec
pub.publish(vec)
pygame.mixer.quit()
pygame.quit()
except rospy.ROSInterruptException:
pass
class ArmCommander(Limb):
"""
This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages
Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation
"""
def __init__(self,
name,
rate=100,
fk='robot',
ik='trac',
default_kv_max=1.,
default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {
'fk': {
'ros': 'compute_fk'
},
'ik': {
'ros':
'compute_ik',
'robot':
'ExternalTools/{}/PositionKinematicsNode/IKService'.format(
name),
'trac':
'trac_ik_{}'.format(name)
}
}
self._kinematics_services = {
'fk': {
'ros': {
'service':
rospy.ServiceProxy(self._kinematics_names['fk']['ros'],
GetPositionFK),
'func':
self._get_fk_ros
},
'kdl': {
'func': self._get_fk_pykdl
},
'robot': {
'func': self._get_fk_robot
}
},
'ik': {
'ros': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['ros'],
GetPositionIK),
'func':
self._get_ik_ros
},
'robot': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['robot'],
SolvePositionIK),
'func':
self._get_ik_robot
},
'trac': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['trac'],
GetConstrainedPositionIK),
'func':
self._get_ik_trac
},
'kdl': {
'func': self._get_ik_pykdl
}
}
}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(
self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(
self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber(
'/robot/limb/{}/collision_detection_state'.format(name),
CollisionDetectionState,
self._cb_collision,
queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name),
DigitalIOState,
self._cb_dig_io,
queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(
self.name)
self.client = SimpleActionClient(action_server_name,
FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher(
"/move_group/display_planned_path",
DisplayTrajectory,
queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
######################################### CALLBACKS #########################################
def _cb_collision(self, msg):
if msg.collision_state:
with self._stop_lock:
self._stop_reason = 'collision'
def _cb_dig_io(self, msg):
if msg.state > 0:
with self._stop_lock:
self._stop_reason = 'cuff'
#############################################################################################
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_name(self):
return self.name + '_gripper'
def group_name(self):
return self.name + '_arm'
def joint_limits(self):
xml_urdf = rospy.get_param('robot_description')
dict_urdf = xmltodict.parse(xml_urdf)
joints_urdf = []
joints_urdf.append([
j['@name'] for j in dict_urdf['robot']['joint']
if j['@name'] in self.joint_names()
])
joints_urdf.append(
[[float(j['limit']['@lower']),
float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint']
if j['@name'] in self.joint_names()])
# reorder the joints limits
return dict(
zip(self.joint_names(), [
joints_urdf[1][joints_urdf[0].index(name)]
for name in self.joint_names()
]))
def get_current_state(self, list_joint_names=[]):
"""
Returns the current RobotState describing all joint states
:param list_joint_names: If not empty, returns only the state of the requested joints
:return: a RobotState corresponding to the current state read on /robot/joint_states
"""
if len(list_joint_names) == 0:
list_joint_names = self.joint_names()
state = RobotState()
state.joint_state.name = list_joint_names
state.joint_state.position = map(self.joint_angle, list_joint_names)
state.joint_state.velocity = map(self.joint_velocity, list_joint_names)
state.joint_state.effort = map(self.joint_effort, list_joint_names)
return state
def get_ik(self, eef_poses, seeds=(), source=None, params=None):
"""
Return IK solutions of this arm's end effector according to the method declared in the constructor
:param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped
:param seeds: a single seed or a list of seeds of type RobotState for each input pose
:param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated)
:param params: dictionary containing optional non-generic IK parameters
:return: a list of RobotState for each input pose in input or a single RobotState
TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None)
Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose
"""
if not isinstance(eef_poses, list) or isinstance(
eef_poses[0], list) and not isinstance(eef_poses[0][0], list):
eef_poses = [eef_poses]
if not seeds:
seeds = []
elif not isinstance(seeds, list):
seeds = [seeds] * len(eef_poses)
input = []
for eef_pose in eef_poses:
if isinstance(eef_pose, list):
input.append(list_to_pose(eef_pose, self._world))
elif isinstance(eef_pose, PoseStamped):
input.append(eef_pose)
else:
raise TypeError(
"ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}"
.format(str(type(eef_pose))))
output = self._kinematics_services['ik'][
self._selected_ik if source is None else source]['func'](input,
seeds,
params)
return output if len(eef_poses) > 1 else output[0]
def get_fk(self, frame_id=None, robot_state=None):
"""
Return The FK solution oth this robot state according to the method declared in the constructor
robot_state = None will give the current endpoint pose in frame_id
:param robot_state: a RobotState message
:param frame_id: the frame you want the endpoint pose into
:return: [[x, y, z], [x, y, z, w]]
"""
if frame_id is None:
frame_id = self._world
if isinstance(robot_state, RobotState) or robot_state is None:
return self._kinematics_services['fk'][self._selected_fk]['func'](
frame_id, robot_state)
else:
raise TypeError(
"ArmCommander.get_fk() accepts only a RobotState, got {}".
format(str(type(robot_state))))
def _get_fk_pykdl(self, frame_id, state=None):
if state is None:
state = self.get_current_state()
fk = self._kinematics_pykdl.forward_position_kinematics(
dict(zip(state.joint_state.name, state.joint_state.position)))
ps = list_to_pose([fk[:3], fk[-4:]], self._world)
return self._tf_listener.transformPose(frame_id, ps)
def _get_fk_robot(self, frame_id=None, state=None):
# Keep this half-working FK, still used by generate_cartesian_path (trajectories.py)
if state is not None:
raise NotImplementedError(
"_get_fk_robot has no FK service provided by the robot except for its current endpoint pose"
)
ps = list_to_pose(self.endpoint_pose(), self._world)
return self._tf_listener.transformPose(frame_id, ps)
def _get_fk_ros(self, frame_id=None, state=None):
rqst = GetPositionFKRequest()
rqst.header.frame_id = self._world if frame_id is None else frame_id
rqst.fk_link_names = [self.endpoint_name()]
if isinstance(state, RobotState):
rqst.robot_state = state
elif isinstance(state, JointState):
rqst.robot_state.joint_state = state
elif state is None:
rqst.robot_state = self.get_current_state()
else:
raise AttributeError("Provided state is an invalid type")
fk_answer = self._kinematics_services['fk']['ros']['service'].call(
rqst)
if fk_answer.error_code.val == 1:
return fk_answer.pose_stamped[0]
else:
return None
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError(
"_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}"
.format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(
pose[0], pose[1], [
seeds[pose_num].joint_state.position[
seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()
] if len(seeds) > 0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def _get_ik_robot(self, eef_poses, seeds=(), params=None):
ik_req = SolvePositionIKRequest()
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
ik_req.seed_mode = ik_req.SEED_USER if len(
seeds) > 0 else ik_req.SEED_CURRENT
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['robot']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(
RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_trac(self, eef_poses, seeds=(), params=None):
ik_req = GetConstrainedPositionIKRequest()
if params is None:
ik_req.num_steps = 1
else:
ik_req.end_tolerance = params['end_tolerance']
ik_req.num_steps = params['num_attempts']
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
if len(seeds) == 0:
seeds = [self.get_current_state()] * len(eef_poses)
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['trac']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(
RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_ros(self, eef_poses, seeds=()):
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = self.group_name()
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
rqst.ik_request.pose_stamped = eef_pose # Do we really to do a separate call for each pose? _vector not used
ik_answer = self._kinematics_services['ik']['ros']['service'].call(
rqst)
if len(seeds) > 0:
rqst.ik_request.robot_state = seeds[pose_num]
if ik_answer.error_code.val == 1:
# Apply a filter to return only joints of this group
try:
ik_answer.solution.joint_state.velocity = [
value for id_joint, value in enumerate(
ik_answer.solution.joint_state.velocity)
if ik_answer.solution.joint_state.name[id_joint] in
self.joint_names()
]
ik_answer.solution.joint_state.effort = [
value for id_joint, value in enumerate(
ik_answer.solution.joint_state.effort)
if ik_answer.solution.joint_state.name[id_joint] in
self.joint_names()
]
except IndexError:
pass
ik_answer.solution.joint_state.position = [
value for id_joint, value in enumerate(
ik_answer.solution.joint_state.position)
if ik_answer.solution.joint_state.name[id_joint] in
self.joint_names()
]
ik_answer.solution.joint_state.name = [
joint for joint in ik_answer.solution.joint_state.name
if joint in self.joint_names()
]
solutions.append(ik_answer.solution)
else:
solutions.append(None)
return solutions
def translate_to_cartesian(self,
path,
frame_id,
time,
n_points=50,
max_speed=np.pi / 4,
min_success_rate=0.5,
display_only=False,
timeout=0,
stop_test=lambda: False,
pause_test=lambda: False):
"""
Translate the end effector in straight line, following path=[translate_x, translate_y, translate_z] wrt frame_id
:param path: Path [x, y, z] to follow wrt frame_id
:param frame_id: frame_id of the given input path
:param time: Time of the generated trajectory
:param n_points: Number of 3D points of the cartesian trajectory
:param max_speed: Maximum speed for every single joint in rad.s-1, allowing to avoid jumps in joints configuration
:param min_success_rate: Raise RuntimeError in case the success rate is lower than min_success_rate
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up (default is 0 = do not retry)
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return:
:raises: RuntimeError if success rate is too low
"""
def trajectory_callable(start):
traj, success_rate = trajectories.generate_cartesian_path(
path, frame_id, time, self, None, n_points, max_speed)
if success_rate < min_success_rate:
raise RuntimeError(
"Unable to generate cartesian path (success rate : {})".
format(success_rate))
return traj
return self._relaunched_move_to(trajectory_callable, display_only,
timeout, stop_test, pause_test)
def move_to_controlled(self,
goal,
rpy=[0, 0, 0],
display_only=False,
timeout=15,
stop_test=lambda: False,
pause_test=lambda: False):
"""
Move to a goal using interpolation in joint space with limitation of velocity and acceleration
:param goal: Pose, PoseStamped or RobotState
:param rpy: Vector [Roll, Pitch, Yaw] filled with 0 if this axis must be preserved, 1 otherwise
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return: None
:raises: ValueError if IK has no solution
"""
assert callable(stop_test)
assert callable(pause_test)
if not isinstance(goal, RobotState):
goal = self.get_ik(goal)
if goal is None:
raise ValueError('This goal is not reachable')
# collect the robot state
start = self.get_current_state()
# correct the orientation if rpy is set
if np.array(rpy).any():
# convert the starting point to rpy pose
pos, rot = states.state_to_pose(start, self, True)
for i in range(3):
if rpy[i]:
rpy[i] = rot[i]
goal = states.correct_state_orientation(goal, rpy, self)
# parameters for trapezoidal method
kv_max = self.kv_max
ka_max = self.ka_max
return self._relaunched_move_to(
lambda start: trajectories.trapezoidal_speed_trajectory(
goal, start=start, kv_max=kv_max, ka_max=ka_max), display_only,
timeout, stop_test, pause_test)
def rotate_joint(self,
joint_name,
goal_angle,
display_only=False,
stop_test=lambda: False,
pause_test=lambda: False):
goal = self.get_current_state()
joint = goal.joint_state.name.index(joint_name)
# JTAS accepts all angles even out of limits
#limits = self.joint_limits()[joint_name]
goal.joint_state.position[joint] = goal_angle
return self.move_to_controlled(goal,
display_only=display_only,
stop_test=stop_test,
pause_test=pause_test)
def _relaunched_move_to(self,
trajectory_callable,
display_only=False,
timeout=15,
stop_test=lambda: False,
pause_test=lambda: False):
"""
Relaunch several times (until cuff interaction or failure) a move_to() whose trajectory is generated by the callable passed in parameter
:param trajectory_callable: Callable to call to recompute the trajectory
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
:return:
"""
assert callable(trajectory_callable)
retry = True
t0 = rospy.get_time()
while retry and rospy.get_time() - t0 < timeout or timeout == 0:
start = self.get_current_state()
trajectory = trajectory_callable(start)
if display_only:
self.display(trajectory)
break
else:
retry = not self.execute(
trajectory, test=lambda: stop_test() or pause_test())
if pause_test():
if not stop_test():
retry = True
while pause_test():
rospy.sleep(0.1)
if timeout == 0:
return not display_only and not retry
if retry:
rospy.sleep(1)
return not display_only and not retry
def get_random_pose(self):
# get joint names
joint_names = self.joint_names()
# create a random joint state
bounds = []
for key, value in self.joint_limits().iteritems():
bounds.append(value)
bounds = np.array(bounds)
joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1],
len(joint_names))
# append it in a robot state
goal = RobotState()
goal.joint_state.name = joint_names
goal.joint_state.position = joint_state
goal.joint_state.header.stamp = rospy.Time.now()
goal.joint_state.header.frame_id = 'base'
return goal
######################## OPERATIONS ON TRAJECTORIES
# TO BE MOVED IN trajectories.py
def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time * (1.0 / nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j], goal_state[j],
nb_points + 1)
joints.append(pose_lin[1:].tolist())
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i + 1) * dt)
# append the position to the message
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(
JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError(
"ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}"
.format(str(type(trajectory))))
self._display_traj.publish(dt)
def execute(self, trajectory, test=None, epsilon=0.1):
"""
Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
Non-blocking needs should deal with the JointTrajectory action server
:param trajectory: either a RobotTrajectory or a JointTrajectory
:param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
:return: True if execution ended successfully, False otherwise
"""
def distance_to_first_point(point):
joint_pos = np.array(point.positions)
return np.linalg.norm(current_array - joint_pos)
self.display(trajectory)
with self._stop_lock:
self._stop_reason = ''
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
elif not isinstance(trajectory, JointTrajectory):
raise TypeError(
"Execute only accept RobotTrajectory or JointTrajectory")
ftg = FollowJointTrajectoryGoal()
ftg.trajectory = trajectory
# check if it is necessary to move to the first point
current = self.get_current_state()
current_array = np.array([
current.joint_state.position[current.joint_state.name.index(joint)]
for joint in trajectory.joint_names
])
if distance_to_first_point(trajectory.points[0]) > epsilon:
# convert first point to robot state
rs = RobotState()
rs.joint_state.name = trajectory.joint_names
rs.joint_state.position = trajectory.points[0].positions
# move to the first point
self.move_to_controlled(rs)
# execute the input trajectory
self.client.send_goal(ftg)
# Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory
while self.client.simple_state != SimpleGoalState.DONE:
if callable(test) and test():
self.client.cancel_goal()
return True
if self._stop_reason != '':
self.client.cancel_goal()
return False
self._rate.sleep()
return True
def close(self):
"""
Open the gripper
:return: True if an object has been grasped
"""
return self._gripper.close(True)
def open(self):
"""
Close the gripper
return: True if an object has been released
"""
return self._gripper.open(True)
def gripping(self):
return self._gripper.gripping()
def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True):
"""
Blocks until external motion is given to the arm
:param threshold:
:param rate: rate of control loop in Hertz
:param ignore_gripping: True if we must wait even if no object is gripped
"""
def detect_variation():
new_effort = np.array(
self.get_current_state(
[self.name + '_w0', self.name + '_w1',
self.name + '_w2']).joint_state.effort)
delta = np.absolute(effort - new_effort)
return np.amax(delta) > threshold
# record the effort at calling time
effort = np.array(
self.get_current_state(
[self.name + '_w0', self.name + '_w1',
self.name + '_w2']).joint_state.effort)
# loop till the detection of changing effort
rate = rospy.Rate(rate)
while not detect_variation() and not rospy.is_shutdown() and (
ignore_gripping or self.gripping()):
rate.sleep()
class BlockStackerNode(object):
RATE = 250
############################################################################
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg',
Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg',
Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state',
Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
############################################################################
def start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)
class BlockStackerNode(object):
RATE = 250
############################################################################
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg', Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg', Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState, self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
############################################################################
def start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)
class Baxterpicknumber():
def __init__(self):
global place_areax
global place_areay
global placetimes
rospy.loginfo("=============Ready to move the robot================")
#Initialize moveit_commander
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
self.left_arm_group = moveit_commander.MoveGroupCommander("left_arm")
#Display Trajectory in RVIZ
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.moving_publisher = rospy.Publisher('amimoving',
String,
queue_size=10)
rospy.sleep(3.0)
self.planning_frame = self.right_arm_group.get_planning_frame()
rospy.loginfo("============ Reference frame: %s" % self.planning_frame)
self.eef_link = self.right_arm_group.get_end_effector_link()
print "============ End effector: %s" % self.eef_link
self.group_names = self.robot.get_group_names()
print "============ Robot Groups:", self.robot.get_group_names()
#set planning and execution parameters
self.right_arm_group.set_goal_position_tolerance(0.01)
self.right_arm_group.set_goal_orientation_tolerance(0.01)
self.right_arm_group.set_planning_time(5.0)
self.right_arm_group.allow_replanning(False)
self.right_arm_group.set_max_velocity_scaling_factor(1)
self.right_arm_group.set_max_acceleration_scaling_factor(1)
self.left_arm_group.set_goal_position_tolerance(0.01)
self.left_arm_group.set_goal_orientation_tolerance(0.01)
self.left_arm_group.set_planning_time(5.0)
self.left_arm_group.allow_replanning(False)
self.left_arm_group.set_max_velocity_scaling_factor(0.5)
self.left_arm_group.set_max_acceleration_scaling_factor(0.5)
#specify which gripper and limb are taking command
self.right_gripper = Gripper('right', CHECK_VERSION)
self.right_gripper.reboot()
self.right_gripper.calibrate()
# self.left_gripper = Gripper('left', CHECK_VERSION)
# self.left_gripper.reboot()
# self.left_gripper.calibrate()
#Standoff hight above the block
self.standoff = 0.2
#Screw axis for Right arm of Baxter
self.Blist = np.array([[-1, 0, 0, 0, 1.17594, 0],
[0, 1, 0, 1.10694, 0, -0.079],
[0, 0, 1, 0, 0.079, 0],
[0, 1, 0, 0.74259, 0, -0.01],
[0, 0, 1, 0, 0.01, 0], [0, 1, 0, 0.3683, 0, 0],
[0, 0, 1, 0, 0, 0]]).T
self.M = np.array(
[[0, 1 / sqrt(2), 1 / sqrt(2), 0.064 + (1.17594 / sqrt(2))],
[0, 1 / sqrt(2), -1 / sqrt(2), -0.278 - (1.17594 / sqrt(2))],
[-1, 0, 0, 0.19135], [0, 0, 0, 1]])
#subscribe to range sensor topic
self.__right_sensor = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range, self.rangecallback)
self.rangestatetemp = Range()
#subscribe to target location topic
self.grabcamera()
self.go_startpose()
self.location_data = rospy.Subscriber('square_location', String,
self.compute_cartesian)
# self.compute_cartesian()
def grabcamera(self):
# self.putcamerapose= {'left_s0': -np.pi/4,
# 'left_s1': -np.pi/12,
# 'left_e0': 0,
# 'left_e1': 0,
# 'left_w0': 0,
# 'left_w1': np.pi/12,
# 'left_w2': 0}
# self.left_arm_group.set_joint_value_target(self.putcamerapose)
# plan1=self.left_arm_group.plan()
# self.left_arm_group.go()
# rospy.loginfo("=======put camera in left gripper please=======")
# raw_input()
# self.left_gripper.close()
# rospy.sleep(3.0)
self.holdcamerapose = {
'left_s0': -0.04678641403050512,
'left_s1': -1.4001409641424114,
'left_e0': 0.3712233506682701,
'left_e1': 1.3940050409908697,
'left_w0': -0.46326219794139495,
'left_w1': 2.05706823655434,
'left_w2': -0.943014689352558
}
self.left_arm_group.set_joint_value_target(self.holdcamerapose)
plan2 = self.left_arm_group.plan()
self.left_arm_group.go()
rospy.sleep(2.0)
def rangecallback(self, data):
self.rangestatetemp = data
self.rangestate = self.rangestatetemp.range
def go_startpose(self):
self.move = "0"
self.moving_publisher.publish(self.move)
global placetimes
# self.moving='1'
# self.moving_publisher(self.moving)
#Natural position of baxter
self.initialpose = {
'right_s0': 0, #0.03413107253045045,
'right_s1': -50 * np.pi / 180, #-0.6937428113211783,
'right_e0': 0, #0.1277039005914607,
'right_e1': 70 * np.pi / 180, #0.9671748867617533,
'right_w0': 0, #-0.16835439147042416,
'right_w1': 72 * np.pi / 180, #1.0810729602622453,
'right_w2': 0
} #-0.1472621556369997}
self.right_arm_group.set_joint_value_target(self.initialpose)
rospy.loginfo("Attempting to start pose")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
# rospy.sleep(1.0)
# if placetimes>0:
# rospy.loginfo("==========Ready to fetch another number, Press 'Enter' to cotinue=======")
# raw_input()
# self.standbypose()
self.standbypose()
def standbypose(self):
global placetimes
if placetimes < 3:
self.placex_coord = place_areax[0]
self.placey_coord = place_areay[placetimes]
elif placetimes < 6:
self.placex_coord = place_areax[1]
self.placey_coord = place_areay[placetimes - 3]
elif placetimes < 9:
self.placex_coord = place_areax[2]
self.placey_coord = place_areay[placetimes - 6]
else:
rospy.loginfo(
"========There is currently no place to put block, Please press 'Enter' after cleaning the table."
)
raw_input()
placetimes = 0
self.placex_coord = place_areax[0]
self.placey_coord = place_areay[placetimes]
#self.compute_cartesian()
def compute_cartesian(self, data):
# self.moving='0'
# self.moving_publisher(self.moving)
# testingdata=np.array([1, 0.55597888,-0.24354399,-0.3,0])
# data=testingdata
data = data.data.split('&')
fetchnumber = float(data[0])
# rospy.loginfo("=============Trying to fetch number " + str(fetchnumber) + " Press 'Enter' to confirm==============")
confirm = raw_input("=============Trying to fetch number " +
str(fetchnumber) +
" Press 'y/n' to confirm==============")
# confirm ='y'
if confirm == "y":
rospy.loginfo("==============Received desired position")
#extract x,y,z coordinate from subscriber data
self.x_coord = float(data[1]) - 0.020
# rospy.loginfo(str(self.x_coord))
self.y_coord = float(data[2]) + 0.038
# rospy.loginfo(str(self.y_coord))
self.z_coord = -0.3
self.theta = 0
self.pose_target = converttoSE3(self.x_coord, self.y_coord,
self.z_coord, self.theta)
# try:
self.move_arm_standoff()
else:
self.go_startpose()
# except:
# self.right_gripper.open()
# self.go_startpose()
def move_arm_standoff(self):
pose_target = converttoSE3(self.x_coord, self.y_coord,
self.z_coord + self.standoff, self.theta)
theta0list = np.array([0.74, -0.67, 0.2, 1.2, 0, 1.068801113959162, 0])
joint_targettemp = IKinBody(self.Blist, self.M, pose_target,
theta0list, 0.01, 0.001)
if joint_targettemp[1] == True:
rospy.loginfo(
"Solution Found! Joint target value computed successfully.")
print(joint_targettemp[0])
self.pickstandoff_joint_target = {
'right_s0': joint_targettemp[0][0], #+0.05,
'right_s1': joint_targettemp[0][1],
'right_e0': joint_targettemp[0][2],
'right_e1': joint_targettemp[0][3],
'right_w0': joint_targettemp[0][4],
'right_w1': joint_targettemp[0][5],
'right_w2': joint_targettemp[0][6]
}
# try:
self.right_arm_group.set_joint_value_target(
self.pickstandoff_joint_target)
rospy.loginfo("Attempting to go to pick stand off position")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
rospy.sleep(1.0)
self.move_arm_pick()
# except:
# self.right_gripper.open()
# self.go_startpose()
else:
rospy.logerr(
"Could not find valid joint target value, returnning to Initial position"
)
self.right_gripper.open()
self.go_startpose()
def move_arm_pick(self):
pose_target = converttoSE3(self.x_coord, self.y_coord, self.z_coord,
self.theta)
theta0list = np.array([
self.pickstandoff_joint_target['right_s0'],
self.pickstandoff_joint_target['right_s1'],
self.pickstandoff_joint_target['right_e0'],
self.pickstandoff_joint_target['right_e1'],
self.pickstandoff_joint_target['right_w0'],
self.pickstandoff_joint_target['right_w1'],
self.pickstandoff_joint_target['right_w2']
])
joint_targettemp = IKinBody(self.Blist, self.M, pose_target,
theta0list, 0.01, 0.001)
if joint_targettemp[1] == True:
rospy.loginfo(
"Solution Found! Joint target value computed successfully.")
print(joint_targettemp[0])
self.pick_joint_target = {
'right_s0': joint_targettemp[0][0],
'right_s1': joint_targettemp[0][1],
'right_e0': joint_targettemp[0][2],
'right_e1': joint_targettemp[0][3],
'right_w0': joint_targettemp[0][4],
'right_w1': joint_targettemp[0][5],
'right_w2': joint_targettemp[0][6]
}
self.right_arm_group.set_joint_value_target(self.pick_joint_target)
rospy.loginfo("Attempting to go to pick position")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
rospy.sleep(1.0)
print(self.rangestate)
if self.rangestate < 0.2:
self.right_gripper.close()
else:
self.right_gripper.open()
self.move_arm_backstandoff()
else:
rospy.logerr(
"Could not find valid joint target value, returnning to Initial position"
)
self.right_gripper.open()
self.go_startpose()
# self.move_arm_backstandoff()
def move_arm_backstandoff(self):
theta0list = np.array([
self.pick_joint_target['right_s0'],
self.pick_joint_target['right_s1'],
self.pick_joint_target['right_e0'],
self.pick_joint_target['right_e1'],
self.pick_joint_target['right_w0'],
self.pick_joint_target['right_w1'],
self.pick_joint_target['right_w2']
])
pose_target = converttoSE3(self.x_coord, self.y_coord,
self.z_coord + self.standoff, self.theta)
joint_targettemp = IKinBody(self.Blist, self.M, pose_target,
theta0list, 0.01, 0.001)
if joint_targettemp[1] == True:
rospy.loginfo(
"Solution Found! Joint target value computed successfully.")
print(joint_targettemp[0])
self.pickbackstandoff_joint_target = {
'right_s0': joint_targettemp[0][0],
'right_s1': joint_targettemp[0][1],
'right_e0': joint_targettemp[0][2],
'right_e1': joint_targettemp[0][3],
'right_w0': joint_targettemp[0][4],
'right_w1': joint_targettemp[0][5],
'right_w2': joint_targettemp[0][6]
}
self.right_arm_group.set_joint_value_target(
self.pickbackstandoff_joint_target)
rospy.loginfo("Attempting to go back to pick standoff position")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
rospy.sleep(1.0)
if self.rangestate < 0.3:
self.right_gripper.close()
self.move_arm_standoff2()
else:
rospy.loginfo(
"=========Sorry, I drop the block, returning to start pose====="
)
self.right_gripper.open()
self.go_startpose()
else:
rospy.logerr(
"Could not find valid joint target value, returnning to Initial position"
)
self.right_gripper.open()
self.go_startpose()
def move_arm_standoff2(self):
theta0list = np.array([
self.pickbackstandoff_joint_target['right_s0'],
self.pickbackstandoff_joint_target['right_s1'],
self.pickbackstandoff_joint_target['right_e0'],
self.pickbackstandoff_joint_target['right_e1'],
self.pickbackstandoff_joint_target['right_w0'],
self.pickbackstandoff_joint_target['right_w1'],
self.pickbackstandoff_joint_target['right_w2']
])
# theta0list=np.array([0.8,-0.64,0.25,1.0400389741863105,0.2,1.068801113959162,0])
pose_target = converttoSE3(self.placex_coord, self.placey_coord,
self.z_coord + self.standoff, self.theta)
joint_targettemp = IKinBody(self.Blist, self.M, pose_target,
theta0list, 0.01, 0.001)
if joint_targettemp[1] == True:
rospy.loginfo(
"Solution Found! Joint target value computed successfully.")
print(joint_targettemp[0])
self.placestandoff_joint_target = {
'right_s0': joint_targettemp[0][0],
'right_s1': joint_targettemp[0][1],
'right_e0': joint_targettemp[0][2],
'right_e1': joint_targettemp[0][3],
'right_w0': joint_targettemp[0][4],
'right_w1': joint_targettemp[0][5],
'right_w2': joint_targettemp[0][6]
}
self.right_arm_group.set_joint_value_target(
self.placestandoff_joint_target)
rospy.loginfo("Attempting to go to place stand off position")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
rospy.sleep(1.0)
if self.rangestate < 0.3:
self.right_gripper.close()
self.move_arm_place()
else:
rospy.loginfo("Sorry, I drop the block, return to start pose")
self.right_gripper.open()
self.go_startpose()
else:
rospy.logerr(
"Could not find valid joint target value, returnning to Initial position"
)
self.right_gripper.open()
self.go_startpose()
# rospy.sleep(1.0)
def move_arm_place(self):
global placetimes
pose_target = converttoSE3(self.placex_coord, self.placey_coord,
self.z_coord, 0)
theta0list = np.array([
self.placestandoff_joint_target['right_s0'],
self.placestandoff_joint_target['right_s1'],
self.placestandoff_joint_target['right_e0'],
self.placestandoff_joint_target['right_e1'],
self.placestandoff_joint_target['right_w0'],
self.placestandoff_joint_target['right_w1'],
self.placestandoff_joint_target['right_w2']
])
joint_targettemp = IKinBody(self.Blist, self.M, pose_target,
theta0list, 0.01, 0.001)
if joint_targettemp[1] == True:
rospy.loginfo(
"Solution Found! Joint target value computed successfully.")
print(joint_targettemp[0])
self.place_joint_target = {
'right_s0': joint_targettemp[0][0],
'right_s1': joint_targettemp[0][1],
'right_e0': joint_targettemp[0][2],
'right_e1': joint_targettemp[0][3],
'right_w0': joint_targettemp[0][4],
'right_w1': joint_targettemp[0][5],
'right_w2': joint_targettemp[0][6]
}
self.right_arm_group.set_joint_value_target(
self.place_joint_target)
rospy.loginfo("Attempting to go to pick position")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
placetimes = placetimes + 1
self.right_gripper.open()
# print(placetimes)
self.move_arm_backstandoff2()
else:
rospy.logerr("Could not find valid joint target value")
self.go_startpose()
def move_arm_backstandoff2(self):
theta0list = np.array([
self.place_joint_target['right_s0'],
self.place_joint_target['right_s1'],
self.place_joint_target['right_e0'],
self.place_joint_target['right_e1'],
self.place_joint_target['right_w0'],
self.place_joint_target['right_w1'],
self.place_joint_target['right_w2']
])
pose_target = converttoSE3(self.placex_coord, self.placey_coord,
self.z_coord + self.standoff, 0)
# theta0list=np.array([0.74,-0.585980660972228,0,1.0400389741863105,0,1.068801113959162,0])
joint_targettemp = IKinBody(self.Blist, self.M, pose_target,
theta0list, 0.01, 0.001)
if joint_targettemp[1] == True:
rospy.loginfo(
"Solution Found! Joint target value computed successfully.")
print(joint_targettemp[0])
self.placebackstandoff_joint_target = {
'right_s0': joint_targettemp[0][0],
'right_s1': joint_targettemp[0][1],
'right_e0': joint_targettemp[0][2],
'right_e1': joint_targettemp[0][3],
'right_w0': joint_targettemp[0][4],
'right_w1': joint_targettemp[0][5],
'right_w2': joint_targettemp[0][6]
}
self.right_arm_group.set_joint_value_target(
self.placebackstandoff_joint_target)
rospy.loginfo("Attempting to go to place stand off position")
plan = self.right_arm_group.plan()
self.right_arm_group.go()
self.right_gripper.open()
self.go_startpose()
else:
rospy.logerr("Could not find valid joint target value")
self.go_startpose()
class Baxterpicknumber():
def __init__(self):
rospy.loginfo("=============Ready to move the robot================")
#Initialize moveit_commander
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
#Display Trajectory in RVIZ
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
rospy.sleep(3.0)
self.planning_frame = self.right_arm_group.get_planning_frame()
rospy.loginfo( "============ Reference frame: %s" % self.planning_frame)
self.eef_link = self.right_arm_group.get_end_effector_link()
print "============ End effector: %s" % self.eef_link
self.group_names = self.robot.get_group_names()
print "============ Robot Groups:", self.robot.get_group_names()
#set planning and execution parameters
self.right_arm_group.set_goal_position_tolerance(0.01)
self.right_arm_group.set_goal_orientation_tolerance(0.01)
self.right_arm_group.set_planning_time(5.0)
self.right_arm_group.allow_replanning(False)
self.right_arm_group.set_max_velocity_scaling_factor(0.5)
self.right_arm_group.set_max_acceleration_scaling_factor(0.5)
#specify which gripper and limb are taking command
self.right_gripper = Gripper('right', CHECK_VERSION)
self.right_gripper.reboot()
self.right_gripper.calibrate()
self.pose_target=Pose()
self.startpose=Pose()
self.standoff=0.2
#Where to place the block
self.place_target=Pose()
self.placex_coord= 0.65
self.placey_coord=0
self.__right_sensor=rospy.Subscriber('/robot/range/right_hand_range/state',Range,self.rangecallback)
# self.location_data=rospy.Subscriber('square_location',String,self.compute_cartesian)
self.compute_cartesian()
self.rangestatetemp=Range()
def rangecallback(self, data):
self.rangestatetemp=data
self.rangestate=self.rangestatetemp.range
def go_startpose(self):
self.initialpose = {'right_s0': 0.03413107253045045,
'right_s1': -0.6937428113211783,
'right_e0': 0.1277039005914607,
'right_e1': 0.9671748867617533,
'right_w0': -0.16835439147042416,
'right_w1': 1.0810729602622453,
'right_w2': -0.1472621556369997}
self.right_arm_group.set_joint_value_target(self.initialpose)
rospy.loginfo("Attempting to start pose")
plan=self.right_arm_group.plan()
self.right_arm_group.go()
self.startpose.position.x=0.693292944176
self.startpose.position.y=-0.81004861946
self.startpose.position.z=0.0914586599661
self.startpose.orientation.x=0.235668914045
self.startpose.orientation.y=0.964651313563
self.startpose.orientation.z=-0.0681154565907
self.startpose.orientation.w=0.0962719625176
def compute_cartesian(self):#,data):
testingdata=np.array([0.738492350508,-0.320147457674,-0.209,np.pi])
data=testingdata
# data=data.data.split('&')
rospy.loginfo("=================Received desired position==============")
#extract x,y,z coordinate from subscriber data
self.x_coord=data[0]
self.y_coord=data[1]
self.z_coord=data[2]
theta= data[3]
#put coordinate data back to arrays
pout=np.array([self.x_coord,self.y_coord,self.z_coord])
block_orientation=tr.quaternion_from_euler(theta,0,0,'sxyz')
#convert orentation and position to Quaternion
quat=Quaternion(*block_orientation)
self.quat_position=Point(*pout)
self.quat_orientation= copy.deepcopy(quat)
self.pose_target.position=Point(*pout)
self.pose_target.orientation=self.quat_orientation
#Quaternian location for place area
placepout=np.array([self.placex_coord,self.placey_coord,self.z_coord])
self.place_target.position=Point(*placepout)
self.place_target.orientation=copy.deepcopy(quat)
def move_arm_standoff(self):
# self.moving='1'
# self.moving_publisher.publish(self.moving)
rospy.loginfo("==================Going to standoff posistion==============")
z_standoff=self.z_coord+self.standoff
waypoints=[]
ite=30
xite=np.linspace(self.startpose.position.x,self.x_coord,ite)
yite=np.linspace(self.startpose.position.y,self.y_coord,ite)
zite=np.linspace(self.startpose.position.z,z_standoff,ite)
# xoite=np.linspace(self.startpose.orientation.x,self.quat_orientation.x,ite)
# yoite=np.linspace(self.startpose.orientation.y,self.quat_orientation.y,ite)
# zoite=np.linspace(self.startpose.orientation.z,self.quat_orientation.z,ite)
# woite=np.linspace(self.startpose.orientation.w,self.quat_orientation.w,ite)
for i in range(ite):
p=copy.deepcopy(self.startpose)
p.position.x=xite[i]
p.position.y=yite[i]
p.position.z=zite[i]
# p.orientation.x=xoite[i]
# p.orientation.y=yoite[i]
# p.orientation.z=zoite[i]
# p.orientation.w=woite[i]
waypoints.append(p)
# print(waypoints)
rospy.sleep(1.0)
self.right_arm_group.set_start_state_to_current_state()
fraction = 0.0
max_attempts = 200
attempts = 0
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.right_arm_group.execute(plan)
else:
rospy.logerr("Could not find valid cartesian path")
self.go_startpose()
# self.move_arm_pick()
def move_arm_pick(self):
rospy.loginfo("====================moving to pick===================")
waypoints=[]
z_standoff=self.z_coord+self.standoff
ite=30
zite=np.linspace(z_standoff,self.z_coord,ite)
waypoints=[]
# print(zite)
for i in range(ite):
p=copy.deepcopy(self.pose_target)
p.position.z=zite[i]
waypoints.append(p)
# print(waypoints)
rospy.sleep(1.0)
self.right_arm_group.set_start_state_to_current_state()
fraction = 0.0
max_attempts = 200
attempts = 0
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.right_arm_group.execute(plan)
else:
rospy.logerr("Could not find valid cartesian path")
# print(self.rangestate)
if self.rangestate < 0.4:
self.right_gripper.close()
else:
self.right_gripper.open()
# self.right_arm_group.stop()
# self.right_arm_group.clear_pose_targets()
# rospy.sleep(1.0)
def move_arm_backstandoff(self):
self.right_gripper.close()
rospy.loginfo('====================moving back to standoff=================')
waypoints=[]
z_standoff=self.z_coord+self.standoff
ite=30
zite=np.linspace(self.z_coord,z_standoff,30)
waypoints=[]
# print(zite)
for i in range(ite):
p=copy.deepcopy(self.pose_target)
p.position.z=zite[i]
waypoints.append(p)
# print(waypoints)
rospy.sleep(1.0)
self.right_arm_group.set_start_state_to_current_state()
fraction = 0.0
max_attempts = 200
attempts = 0
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.right_arm_group.execute(plan)
else:
rospy.logerr("Could not find valid cartesian path")
def move_arm_standoff2(self):
rospy.loginfo('==================moving to place standoff position==================')
waypoints=[]
z_standoff=self.z_coord+self.standoff
ite=30
xite=np.linspace(self.x_coord,self.placex_coord,ite)
yite=np.linspace(self.y_coord,self.placey_coord,ite)
waypoints=[]
# print(zite)
for i in range(ite):
p=copy.deepcopy(self.place_target)
p.position.z=z_standoff
p.position.x=xite[i]
p.position.y=yite[i]
waypoints.append(p)
# print(waypoints)
rospy.sleep(1.0)
self.right_arm_group.set_start_state_to_current_state()
fraction = 0.0
max_attempts = 200
attempts = 0
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.right_arm_group.execute(plan)
else:
rospy.logerr("Could not find valid cartesian path")
def move_arm_place(self):
rospy.loginfo('===================moving to place the block===================')
waypoints=[]
z_standoff=self.z_coord+self.standoff
ite=30
zite=np.linspace(z_standoff,self.z_coord,ite)
waypoints=[]
# print(zite)
for i in range(ite):
p=copy.deepcopy(self.place_target)
p.position.z=zite[i]
waypoints.append(p)
# print(waypoints)
rospy.sleep(1.0)
self.right_arm_group.set_start_state_to_current_state()
fraction = 0.0
max_attempts = 200
attempts = 0
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.right_arm_group.execute(plan)
else:
rospy.logerr("Could not find valid cartesian path")
self.right_gripper.open()
def move_arm_backstandoff2(self):
rospy.loginfo('=============moving to place standoff position=================')
waypoints=[]
z_standoff=self.z_coord+self.standoff
ite=30
zite=np.linspace(self.z_coord,z_standoff,ite)
waypoints=[]
# print(zite)
for i in range(ite):
p=copy.deepcopy(self.place_target)
p.position.z=zite[i]
waypoints.append(p)
# print(waypoints)
rospy.sleep(1.0)
self.right_arm_group.set_start_state_to_current_state()
fraction = 0.0
max_attempts = 200
attempts = 0
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.right_arm_group.execute(plan)
else:
rospy.logerr("Could not find valid cartesian path")
self.go_startpose()
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed to IK service: %s" % (e))
print "Service call failed"
print resp
traj = Trajectory('right')
traj.add_point(current_angles, 0.0)
traj.add_point(resp.joints[0].position, 3.0)
traj.add_point(resp.joints[1].position, 6.0)
traj.start()
traj.wait(12.0)
right_gripper.close()
current_angles = [
right_limb.joint_angle(joint) for joint in right_limb.joint_names()
]
traj.clear('right')
traj.add_point(current_angles, 0)
traj.add_point(resp.joints[2].position, 2.5)
traj.start()
traj.wait(2.5)
#rospy.spin()
class ArmCommander(Limb):
"""
This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages
Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation
"""
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {'fk': {'ros': 'compute_fk'},
'ik': {'ros': 'compute_ik',
'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name),
'trac': 'trac_ik_{}'.format(name)}}
self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK),
'func': self._get_fk_ros},
'kdl': {'func': self._get_fk_pykdl},
'robot': {'func': self._get_fk_robot}},
'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK),
'func': self._get_ik_ros},
'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK),
'func': self._get_ik_robot},
'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK),
'func': self._get_ik_trac},
'kdl': {'func': self._get_ik_pykdl}}}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name)
self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
######################################### CALLBACKS #########################################
def _cb_collision(self, msg):
if msg.collision_state:
with self._stop_lock:
self._stop_reason = 'collision'
def _cb_dig_io(self, msg):
if msg.state > 0:
with self._stop_lock:
self._stop_reason = 'cuff'
#############################################################################################
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_name(self):
return self.name+'_gripper'
def group_name(self):
return self.name+'_arm'
def joint_limits(self):
xml_urdf = rospy.get_param('robot_description')
dict_urdf = xmltodict.parse(xml_urdf)
joints_urdf = []
joints_urdf.append([j['@name'] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()])
joints_urdf.append([[float(j['limit']['@lower']), float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()])
# reorder the joints limits
return dict(zip(self.joint_names(),
[joints_urdf[1][joints_urdf[0].index(name)] for name in self.joint_names()]))
def get_current_state(self, list_joint_names=[]):
"""
Returns the current RobotState describing all joint states
:param list_joint_names: If not empty, returns only the state of the requested joints
:return: a RobotState corresponding to the current state read on /robot/joint_states
"""
if len(list_joint_names) == 0:
list_joint_names = self.joint_names()
state = RobotState()
state.joint_state.name = list_joint_names
state.joint_state.position = map(self.joint_angle, list_joint_names)
state.joint_state.velocity = map(self.joint_velocity, list_joint_names)
state.joint_state.effort = map(self.joint_effort, list_joint_names)
return state
def get_ik(self, eef_poses, seeds=(), source=None, params=None):
"""
Return IK solutions of this arm's end effector according to the method declared in the constructor
:param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped
:param seeds: a single seed or a list of seeds of type RobotState for each input pose
:param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated)
:param params: dictionary containing optional non-generic IK parameters
:return: a list of RobotState for each input pose in input or a single RobotState
TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None)
Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose
"""
if not isinstance(eef_poses, list) or isinstance(eef_poses[0], list) and not isinstance(eef_poses[0][0], list):
eef_poses = [eef_poses]
if not seeds:
seeds=[]
elif not isinstance(seeds, list):
seeds = [seeds]*len(eef_poses)
input = []
for eef_pose in eef_poses:
if isinstance(eef_pose, list):
input.append(list_to_pose(eef_pose, self._world))
elif isinstance(eef_pose, PoseStamped):
input.append(eef_pose)
else:
raise TypeError("ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}".format(str(type(eef_pose))))
output = self._kinematics_services['ik'][self._selected_ik if source is None else source]['func'](input, seeds, params)
return output if len(eef_poses)>1 else output[0]
def get_fk(self, frame_id=None, robot_state=None):
"""
Return The FK solution oth this robot state according to the method declared in the constructor
robot_state = None will give the current endpoint pose in frame_id
:param robot_state: a RobotState message
:param frame_id: the frame you want the endpoint pose into
:return: [[x, y, z], [x, y, z, w]]
"""
if frame_id is None:
frame_id = self._world
if isinstance(robot_state, RobotState) or robot_state is None:
return self._kinematics_services['fk'][self._selected_fk]['func'](frame_id, robot_state)
else:
raise TypeError("ArmCommander.get_fk() accepts only a RobotState, got {}".format(str(type(robot_state))))
def _get_fk_pykdl(self, frame_id, state=None):
if state is None:
state = self.get_current_state()
fk = self._kinematics_pykdl.forward_position_kinematics(dict(zip(state.joint_state.name, state.joint_state.position)))
return [fk[:3], fk[-4:]]
def _get_fk_robot(self, frame_id = None, state=None):
# Keep this half-working FK, still used by generate_cartesian_path (trajectories.py)
if state is not None:
raise NotImplementedError("_get_fk_robot has no FK service provided by the robot except for its current endpoint pose")
ps = list_to_pose(self.endpoint_pose(), self._world)
return self._tf_listener.transformPose(frame_id, ps)
def _get_fk_ros(self, frame_id = None, state=None):
rqst = GetPositionFKRequest()
rqst.header.frame_id = self._world if frame_id is None else frame_id
rqst.fk_link_names = [self.endpoint_name()]
if isinstance(state, RobotState):
rqst.robot_state = state
elif isinstance(state, JointState):
rqst.robot_state.joint_state = state
elif state is None:
rqst.robot_state = self.get_current_state()
else:
raise AttributeError("Provided state is an invalid type")
fk_answer = self._kinematics_services['fk']['ros']['service'].call(rqst)
if fk_answer.error_code.val==1:
return fk_answer.pose_stamped[0]
else:
return None
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1],
[seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()] if len(seeds)>0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def _get_ik_robot(self, eef_poses, seeds=(), params=None):
ik_req = SolvePositionIKRequest()
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
ik_req.seed_mode = ik_req.SEED_USER if len(seeds) > 0 else ik_req.SEED_CURRENT
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['robot']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_trac(self, eef_poses, seeds=(), params=None):
ik_req = GetConstrainedPositionIKRequest()
if params is None:
ik_req.num_steps = 1
else:
ik_req.end_tolerance = params['end_tolerance']
ik_req.num_steps = params['num_attempts']
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
if len(seeds) == 0:
seeds = [self.get_current_state()]*len(eef_poses)
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['trac']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def _get_ik_ros(self, eef_poses, seeds=()):
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = self.group_name()
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
rqst.ik_request.pose_stamped = eef_pose # Do we really to do a separate call for each pose? _vector not used
ik_answer = self._kinematics_services['ik']['ros']['service'].call(rqst)
if len(seeds) > 0:
rqst.ik_request.robot_state = seeds[pose_num]
if ik_answer.error_code.val==1:
# Apply a filter to return only joints of this group
try:
ik_answer.solution.joint_state.velocity = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.velocity) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()]
ik_answer.solution.joint_state.effort = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.effort) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()]
except IndexError:
pass
ik_answer.solution.joint_state.position = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.position) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()]
ik_answer.solution.joint_state.name = [joint for joint in ik_answer.solution.joint_state.name if joint in self.joint_names()]
solutions.append(ik_answer.solution)
else:
solutions.append(None)
return solutions
def translate_to_cartesian(self, path, frame_id, time, n_points=50, max_speed=np.pi/4, min_success_rate=0.5, display_only=False,
timeout=0, stop_test=lambda:False, pause_test=lambda:False):
"""
Translate the end effector in straight line, following path=[translate_x, translate_y, translate_z] wrt frame_id
:param path: Path [x, y, z] to follow wrt frame_id
:param frame_id: frame_id of the given input path
:param time: Time of the generated trajectory
:param n_points: Number of 3D points of the cartesian trajectory
:param max_speed: Maximum speed for every single joint in rad.s-1, allowing to avoid jumps in joints configuration
:param min_success_rate: Raise RuntimeError in case the success rate is lower than min_success_rate
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up (default is 0 = do not retry)
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return:
:raises: RuntimeError if success rate is too low
"""
def trajectory_callable(start):
traj, success_rate = trajectories.generate_cartesian_path(path, frame_id, time, self, None, n_points, max_speed)
if success_rate < min_success_rate:
raise RuntimeError("Unable to generate cartesian path (success rate : {})".format(success_rate))
return traj
return self._relaunched_move_to(trajectory_callable, display_only, timeout, stop_test, pause_test)
def move_to_controlled(self, goal, rpy=[0, 0, 0], display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False):
"""
Move to a goal using interpolation in joint space with limitation of velocity and acceleration
:param goal: Pose, PoseStamped or RobotState
:param rpy: Vector [Roll, Pitch, Yaw] filled with 0 if this axis must be preserved, 1 otherwise
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
/!\ Test functions must be fast, they will be called at 100Hz!
:return: None
:raises: ValueError if IK has no solution
"""
assert callable(stop_test)
assert callable(pause_test)
if not isinstance(goal, RobotState):
goal = self.get_ik(goal)
if goal is None:
raise ValueError('This goal is not reachable')
# collect the robot state
start = self.get_current_state()
# correct the orientation if rpy is set
if np.array(rpy).any():
# convert the starting point to rpy pose
pos, rot = states.state_to_pose(start,
self,
True)
for i in range(3):
if rpy[i]:
rpy[i] = rot[i]
goal = states.correct_state_orientation(goal, rpy, self)
# parameters for trapezoidal method
kv_max = self.kv_max
ka_max = self.ka_max
return self._relaunched_move_to(lambda start: trajectories.trapezoidal_speed_trajectory(goal, start=start ,kv_max=kv_max, ka_max=ka_max),
display_only, timeout, stop_test, pause_test)
def rotate_joint(self, joint_name, goal_angle, display_only=False, stop_test=lambda:False, pause_test=lambda:False):
goal = self.get_current_state()
joint = goal.joint_state.name.index(joint_name)
# JTAS accepts all angles even out of limits
#limits = self.joint_limits()[joint_name]
goal.joint_state.position[joint] = goal_angle
return self.move_to_controlled(goal, display_only=display_only, stop_test=stop_test, pause_test=pause_test)
def _relaunched_move_to(self, trajectory_callable, display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False):
"""
Relaunch several times (until cuff interaction or failure) a move_to() whose trajectory is generated by the callable passed in parameter
:param trajectory_callable: Callable to call to recompute the trajectory
:param display_only:
:param timeout: In case of cuff interaction, indicates the max time to retry before giving up
:param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal
:return:
"""
assert callable(trajectory_callable)
retry = True
t0 = rospy.get_time()
while retry and rospy.get_time()-t0 < timeout or timeout == 0:
start = self.get_current_state()
trajectory = trajectory_callable(start)
if display_only:
self.display(trajectory)
break
else:
retry = not self.execute(trajectory, test=lambda: stop_test() or pause_test())
if pause_test():
if not stop_test():
retry = True
while pause_test():
rospy.sleep(0.1)
if timeout == 0:
return not display_only and not retry
if retry:
rospy.sleep(1)
return not display_only and not retry
def get_random_pose(self):
# get joint names
joint_names = self.joint_names()
# create a random joint state
bounds = []
for key, value in self.joint_limits().iteritems():
bounds.append(value)
bounds = np.array(bounds)
joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1], len(joint_names))
# append it in a robot state
goal = RobotState()
goal.joint_state.name = joint_names
goal.joint_state.position = joint_state
goal.joint_state.header.stamp = rospy.Time.now()
goal.joint_state.header.frame_id = 'base'
return goal
######################## OPERATIONS ON TRAJECTORIES
# TO BE MOVED IN trajectories.py
def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time*(1.0/nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1)
joints.append(pose_lin[1:].tolist())
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i+1)*dt)
# append the position to the message
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory))))
self._display_traj.publish(dt)
def execute(self, trajectory, test=None, epsilon=0.1):
"""
Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
Non-blocking needs should deal with the JointTrajectory action server
:param trajectory: either a RobotTrajectory or a JointTrajectory
:param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
:return: True if execution ended successfully, False otherwise
"""
def distance_to_first_point(point):
joint_pos = np.array(point.positions)
return np.linalg.norm(current_array - joint_pos)
self.display(trajectory)
with self._stop_lock:
self._stop_reason = ''
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
elif not isinstance(trajectory, JointTrajectory):
raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
ftg = FollowJointTrajectoryGoal()
ftg.trajectory = trajectory
# check if it is necessary to move to the first point
current = self.get_current_state()
current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])
if distance_to_first_point(trajectory.points[0]) > epsilon:
# convert first point to robot state
rs = RobotState()
rs.joint_state.name = trajectory.joint_names
rs.joint_state.position = trajectory.points[0].positions
# move to the first point
self.move_to_controlled(rs)
# execute the input trajectory
self.client.send_goal(ftg)
# Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory
while self.client.simple_state != SimpleGoalState.DONE:
if callable(test) and test():
self.client.cancel_goal()
return True
if self._stop_reason!='':
self.client.cancel_goal()
return False
self._rate.sleep()
return True
def close(self):
"""
Open the gripper
:return: True if an object has been grasped
"""
return self._gripper.close(True)
def open(self):
"""
Close the gripper
return: True if an object has been released
"""
return self._gripper.open(True)
def gripping(self):
return self._gripper.gripping()
def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True):
"""
Blocks until external motion is given to the arm
:param threshold:
:param rate: rate of control loop in Hertz
:param ignore_gripping: True if we must wait even if no object is gripped
"""
def detect_variation():
new_effort = np.array(self.get_current_state([self.name+'_w0',
self.name+'_w1',
self.name+'_w2']).joint_state.effort)
delta = np.absolute(effort - new_effort)
return np.amax(delta) > threshold
# record the effort at calling time
effort = np.array(self.get_current_state([self.name+'_w0',
self.name+'_w1',
self.name+'_w2']).joint_state.effort)
# loop till the detection of changing effort
rate = rospy.Rate(rate)
while not detect_variation() and not rospy.is_shutdown() and (ignore_gripping or self.gripping()):
rate.sleep()
from baxter_interface import Gripper
right_gripper = Gripper('right')
left_gripper = Gripper('left')
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("chatter", String, callback)
# key: (function, args, description)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
right_gripper.close(5.0)
if __name__ == '__main__':
listener()
def main():
"""
Main Script
"""
# Setting up head camera
head_camera = CameraController("left_hand_camera")
head_camera.resolution = (1280, 800)
head_camera.open()
print("setting balance")
happy = False
while not happy:
e = head_camera.exposure
print("exposue value: " + str(e))
e_val = int(input("new exposure value"))
head_camera.exposure = e_val
satisfaction = str(raw_input("satified? y/n"))
happy = 'y' == satisfaction
planner = PathPlanner("right_arm")
listener = tf.TransformListener()
grip = Gripper('right')
grip.calibrate()
rospy.sleep(3)
grip.open()
# creating the table obstacle so that Baxter doesn't hit it
table_size = np.array([.5, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
thickness = 1
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -.112 - thickness / 2
table_size = np.array([.5, 1, thickness])
planner.add_box_obstacle(table_size, "table", table_pose)
raw_input("gripper close")
grip.close()
# ###load cup positions found using cup_detection.py ran in virtual environment
# start_pos_xy = np.load(POURING_CUP_PATH)
# goal_pos_xy = np.load(RECEIVING_CUP_PATH)
# start_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# goal_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# #### END LOADING CUP POSITIONS
camera_subscriber = rospy.Subscriber("cameras/left_hand_camera/image", Image, get_img)
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
cam_pos= [0.188, -0.012, 0.750]
##
## Add the obstacle to the planning scene here
##
# Create a path constraint for the arm
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;
horizontal = getQuaternion(np.array([0,1,0]), np.pi)
z_rot_pos = getQuaternion(np.array([0,0,1]), np.pi / 2)
orig = quatMult(z_rot_pos, horizontal)
orig = getQuaternion(np.array([0,1,0]), np.pi / 2)
#IN THE VIEW OF THE CAMERA
#CORNER1--------->CORNER2
# | |
# | |
# | |
#CORNER3 ------------|
width = 0.3
length = 0.6
CORNER1 = np.array([0.799, -0.524, -0.03])
CORNER2 = CORNER1 + np.array([-width, 0, 0])
CORNER3 = CORNER1 + np.array([0, length, 0])
#CREATE THE GRID
dir1 = CORNER2 - CORNER1
dir2 = CORNER3 - CORNER1
grid_vals = []
ret_vals = []
for i in range(0, 3):
for j in range(0, 4):
grid = CORNER1 + i * dir1 / 2 + j * dir2 / 3
grid_vals.append(grid)
ret_vals.append(np.array([grid[0], grid[1], OBJECT_HEIGHT]))
i = -1
while not rospy.is_shutdown():
for g in grid_vals:
i += 1
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = g[0]
goal_1.pose.position.y = g[1]
goal_1.pose.position.z = g[2]
y = - g[1] + cam_pos[1]
x = g[0] - cam_pos[0]
q = orig
#Orientation as a quaternion
goal_1.pose.orientation.x = q[1][0]
goal_1.pose.orientation.y = q[1][1]
goal_1.pose.orientation.z = q[1][2]
goal_1.pose.orientation.w = q[0]
plan = planner.plan_to_pose(goal_1, [])
if planner.execute_plan(plan):
# raise Exception("Execution failed")
rospy.sleep(1)
#grip.open()
raw_input("open")
rospy.sleep(1)
# plt.imshow(camera_image)
print("yay: " + str(i))
pos = listener.lookupTransform("/reference/right_gripper", "/reference/base", rospy.Time(0))[0]
print("move succesfully to " + str(pos))
fname = os.path.join(IMAGE_DIR, "calib_{}.jpg".format(i))
skimage.io.imsave(fname, camera_image)
print(e)
print("index: ", i)
else:
break
print(np.array(ret_vals))
# save the positions of the gripper so that the homography matrix can be calculated
np.save(POINTS_DIR, np.array(ret_vals))
print(np.load(POINTS_DIR))
break
def __init__(self):
pygame.init()
rospy.init_node('node_mouse', anonymous=True)
right_gripper=Gripper('right')
left_gripper=Gripper('left')
limb_0=baxter_interface.Limb('right')
limb_0.set_joint_positions({'right_w2': 0.00})
def set_j(joint_name):
global delta
limb=baxter_interface.Limb('right')
current_position = limb.joint_angle(joint_name)
send=current_position+delta
if(send>2.80 or send<-2.80):
delta=-delta
time.sleep(0.15)
joint_command = {joint_name: send}
limb.set_joint_positions(joint_command)
#if(current_position-send>0):
# delta=-1
print(current_position)
print(delta)
pygame.display.set_caption('Game')
pygame.mouse.set_visible(True)
vec = Vector3(1, 2, 3)
right_gripper.calibrate()
left_gripper.calibrate()
try:
pub = rospy.Publisher('mouse', Vector3, queue_size=1)
rate = rospy.Rate(10) # 10hz
screen = pygame.display.set_mode((640,480), 0, 32)
pygame.mixer.init()
a=0 ,0 ,0
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEBUTTONUP:
a = pygame.mouse.get_pressed()
print a
if a[0]==1:
left_gripper.open()
else:
left_gripper.close()
if a[2]==1:
right_gripper.open()
else:
right_gripper.close()
if a[1]==1:
set_j('right_w2')
vec.x=a[0]
vec.y=a[1]
vec.z=a[2]
pub.publish(vec)
pygame.mixer.quit()
pygame.quit()
except rospy.ROSInterruptException:
pass
def put(object, count):
tf_listener = tf.TransformListener()
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
gripper_left = Gripper("left")
gripper_right = Gripper("right")
group_left = moveit_commander.MoveGroupCommander("left_arm")
group_right = moveit_commander.MoveGroupCommander("right_arm")
group_left.set_planner_id('RRTConnectkConfigDefault')
group_right.set_planner_id('RRTConnectkConfigDefault')
# parameters
no_detection = False
drop_lemon = True
press_faucet = False
place_cup = False
gripper_position_threshold = 4.6
# count = 3 # number of lemon slices
# clean the scene
# scene.remove_world_object()
# forward kinematic object
kin = baxter_kinematics('left')
while True:
rospy.sleep(1)
# initial pose
initial_pose = Pose()
# initial_pose.orientation.x = 0.9495
# initial_pose.orientation.y = 0.017127
# initial_pose.orientation.z = -0.31322
# initial_pose.orientation.w = 0.0071202
# initial_pose.orientation.x = 0.37
# initial_pose.orientation.y = 0.93
# initial_pose.orientation.z = 0.0
# initial_pose.orientation.w = 0.0
initial_pose.orientation.y = 1.0
initial_pose.position.x = 0.0
initial_pose.position.y = 0.88
initial_pose.position.z = 0.214
rospy.sleep(1)
execute_valid_plan(group_left, initial_pose)
if count == 0:
rospy.loginfo("Finish the task")
break
if no_detection:
rospy.loginfo("No detection")
break
rospy.loginfo("Time to kill the process")
start_time = rospy.get_time()
while (rospy.get_time() - start_time < 3.0):
if rospy.is_shutdown():
no_detection = True
press_faucet = False
break
rospy.loginfo("Looking for the detected pose")
start_time = rospy.get_time()
while not rospy.is_shutdown():
if(rospy.get_time() - start_time > 4.0):
no_detection = True
break
try:
tf_listener.waitForTransform('/base', '/circle', rospy.Time(0), rospy.Duration(1.0))
break
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
continue
if not no_detection:
time_delay = rospy.get_time() - tf_listener.getLatestCommonTime('/circle' , '/base').to_sec()
rospy.loginfo("The time delay for the detection is: %f sec" , time_delay)
if(time_delay > 3.0):
rospy.loginfo("Time delay of the detection exceeds 2.0 second. Stop the process")
break
trans, rot = tf_listener.lookupTransform('/base', '/circle', rospy.Time(0))
rospy.loginfo(trans)
# move to the position that is above the lime
above_pose = get_pose_with_sleep(group_left)
above_pose.pose.position.x = trans[0]
above_pose.pose.position.y = trans[1]
#plan = plan_with_orientation_lock(group_left, above_pose, side = "left", tolerance=1000)
#group_left.execute(plan)
execute_valid_plan(group_left, above_pose)
# set the orientation constraint during the approach
pick_pose = get_pose_with_sleep(group_left)
# pick_pose.pose.position.x = trans[0]
# pick_pose.pose.position.y = trans[1]
pick_pose.pose.position.z = -0.025 # -0.08 for the table
#plan = plan_with_orientation_lock(group_left, pick_pose, side = "left", tolerance=1000)
#group_left.execute(plan)
execute_valid_plan(group_left, pick_pose, dx = 0.02, dy = 0.02)
gripper_left.close()
rospy.sleep(1)
print gripper_left.position()
if gripper_left.position() > gripper_position_threshold:
count-=1
if drop_lemon:
initial_pose = get_pose_with_sleep(group_left)
lift_pose = get_pose_with_sleep(group_left)
lift_pose.pose.position.z += 0.35
# plan = plan_with_orientation_lock(group_left, lift_pose, side = "left")
# group_left.execute(plan)
execute_valid_plan(group_left, lift_pose, dx = 0.05, dy = 0.05)
pre_drop_pose = get_pose_with_sleep(group_left)
pre_drop_pose.pose.position.x = 0.75
pre_drop_pose.pose.position.y = 0.5
# execute_valid_plan(group_left, pre_drop_pose)
drop_pose = get_pose_with_sleep(group_left)
drop_pose.pose.position.x = 0.76
drop_pose.pose.position.y = 0.00
drop_pose.pose.position.z += 0.05
# plan = plan_with_orientation_lock(group_left, drop_pose, side = "left", threshold= 1)
# group_left.execute(plan)
execute_valid_plan(group_left, drop_pose)
gripper_left.open()
rospy.sleep(1)
# execute_valid_plan(group_left, pre_drop_pose)
lift_pose2 = get_pose_with_sleep(group_left)
lift_pose2.pose.position.x = lift_pose.pose.position.x
lift_pose2.pose.position.y = lift_pose.pose.position.y
lift_pose2.pose.position.z = lift_pose.pose.position.z
# plan = plan_with_orientation_lock(group_left, lift_pose2, side = "left", threshold= 1)
# group_left.execute(plan)
execute_valid_plan(group_left, lift_pose)
# fake motion
else:
initial_pose = get_pose_with_sleep(group_left)
lift_pose = get_pose_with_sleep(group_left)
lift_pose.pose.position.z += 0.1
execute_valid_plan(group_left, lift_pose, dx = 0.01, dy = 0.01)
#plan = plan_with_orientation_lock(group_left, lift_pose, side = "left")
#group_left.execute(plan)
execute_valid_plan(group_left, initial_pose,dx = 0.01, dy = 0.01)
gripper_left.open()
rospy.sleep(1)
else:
gripper_left.open()
rospy.sleep(1)
