def main():
"""RSDK Gripper Example: Action Client
Demonstrates creating a client of the Gripper Action Server,
which enables sending commands of standard action type
control_msgs/GripperCommand.
The example will command the grippers to a number of positions
while specifying moving force or vacuum sensor threshold. Be sure
to start Baxter's gripper_action_server before running this example.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument('-g',
'--gripper',
dest='gripper',
required=True,
choices=['left', 'right'],
help='which gripper to send action commands')
args = parser.parse_args(rospy.myargv()[1:])
gripper = args.gripper
print("Initializing node... ")
rospy.init_node("rsdk_gripper_action_client_%s" % (gripper, ))
print("Getting robot state... ")
rs = rbt_baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
print("Running. Ctrl-c to quit")
gc = GripperClient(gripper)
gc.command(position=0.01, effort=90.0)
print gc.wait()
print "Exiting - Gripper Action Test Example Complete"
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-s',
'--state',
const='state',
dest='actions',
action='append_const',
help='Print current robot state')
parser.add_argument('-e',
'--enable',
const='enable',
dest='actions',
action='append_const',
help='Enable the robot')
parser.add_argument('-d',
'--disable',
const='disable',
dest='actions',
action='append_const',
help='Disable the robot')
parser.add_argument('-r',
'--reset',
const='reset',
dest='actions',
action='append_const',
help='Reset the robot')
parser.add_argument('-S',
'--stop',
const='stop',
dest='actions',
action='append_const',
help='Stop the robot')
args = parser.parse_args(rospy.myargv()[1:])
if args.actions == None:
parser.print_usage()
parser.exit(0, "No action defined")
rospy.init_node('rsdk_robot_enable')
rs = rbt_baxter_interface.RobotEnable(CHECK_VERSION)
try:
for act in args.actions:
if act == 'state':
print rs.state()
elif act == 'enable':
rs.enable()
elif act == 'disable':
rs.disable()
elif act == 'reset':
rs.reset()
elif act == 'stop':
rs.stop()
except Exception, e:
rospy.logerr(e.strerror)
def main():
"""RSDK Joint Trajectory Example: Simple Action Client
Creates a client of the Joint Trajectory Action Server
to send commands of standard action type,
control_msgs/FollowJointTrajectoryAction.
Make sure to start the joint_trajectory_action_server.py
first. Then run this example on a specified limb to
command a short series of trajectory points for the arm
to follow.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
args = parser.parse_args(rospy.myargv()[1:])
limb = args.limb
print("Initializing node... ")
rospy.init_node("rsdk_joint_trajectory_client_%s" % (limb,))
print("Getting robot state... ")
rs = rbt_baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
print("Running. Ctrl-c to quit")
positions = {
'left': [-0.11, -0.62, -1.15, 1.32, 0.80, 1.27, 2.39],#[1.10, 0.37191,-1.49, 1.447, -1.178, -1.36, 1.99], #,
'right': [0.11, -0.62, 1.15, 1.32, -0.80, 1.27, -2.39],
}
traj = Trajectory(limb)
# rospy.on_shutdown(traj.stop)
# Command Current Joint Positions first
limb_interface = rbt_baxter_interface.limb.Limb(limb)
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
traj.add_point(current_angles, 0.0)
p1 = positions[limb]
traj.add_point(p1, 10.0)
# traj.add_point([x * 0.75 for x in p1], 9.0)
# traj.add_point([x * 1.25 for x in p1], 12.0)
traj.start()
traj.wait(100.0)
print("Exiting - Joint Trajectory Action Test Complete")
def __init__(self,
limb,
reconfig_server,
rate=100.0,
mode='position_w_id'):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._limb = rbt_baxter_interface.Limb(limb)
self._enable = rbt_baxter_interface.RobotEnable()
self._name = limb
# self._cuff = rbt_baxter_interface.DigitalIO('%s_lower_cuff' % (limb,))
# self._cuff.state_changed.connect(self._cuff_cb)
# Verify joint control mode
self._mode = mode
if (self._mode != 'position' and self._mode != 'position_w_id'
and self._mode != 'velocity'):
rospy.logerr("%s: Action Server Creation Failed - "
"Provided Invalid Joint Control Mode '%s' (Options: "
"'position_w_id', 'position', 'velocity')" % (
self._action_name,
self._mode,
))
return
self._server.start()
self._alive = True
# self._cuff_state = False
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._limb.joint_names():
self._pid[joint] = rbt_baxter_control.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._limb.joint_names())
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._pub_rate.publish(self._control_rate)
self._pub_ff_cmd = rospy.Publisher(self._ns +
'/inverse_dynamics_command',
JointTrajectoryPoint,
tcp_nodelay=True,
queue_size=1)
def __init__(self):
rospy.init_node('moveit_demo_baxter', anonymous=True)
# Initialize the move_group API
# moveit_commander.roscpp_initialize(sys.argv)
moveit_commander.roscpp_initialize(
'/joint_states:=/robot/joint_states')
# Initialize the ROS node
robot = moveit_commander.RobotCommander()
rbt_baxter_interface.RobotEnable().enable()
rospy.sleep(2.0)
#
# GRIPPER_OPEN = [0.05]
# GRIPPER_CLOSED = [-0.03]
# GRIPPER_NEUTRAL = [0.01]
#
# joint_positions = [1.10, 0.37191, -1.49, 1.447, -1.178, -1.36, 1.99]
# left_arm.set_joint_value_target(joint_positions)
# #
# # # Plan and execute the motion
# left_arm.go()
# rospy.sleep(1)
#
# # Connect to the left_gripper move group
# pose = 0.02/1.0
# GRIPPER_NEUTRAL =[pose,-pose]
left_arm = moveit_commander.MoveGroupCommander('left_arm')
left_gripper = moveit_commander.MoveGroupCommander('left_hand')
# #
# # # Get the name of the end-effector link
end_effector_link = left_arm.get_end_effector_link()
#
# # Display the name of the end_effector link
# # rospy.loginfo("The end effector link is: " + str(end_effector_link))
#
# # Set a small tolerance on joint angles
# left_arm.set_goal_joint_tolerance(0.001)
# # left_gripper.set_goal_joint_tolerance(0.001)
#
# # Start the arm target in "resting" pose stored in the SRDF file
# left_arm.set_named_target('left_neutral')
#
# # Plan a trajectory to the goal configuration
# traj = left_arm.plan()
#
# # Execute the planned trajectory
# left_arm.execute(traj)
#
# # Pause for a moment
# rospy.sleep(1)
#
# # Set the gripper target to neutal position using a joint value target
a = left_gripper.get_active_joints()
b = left_gripper.get_joints()
print(a, b)
left_gripper.set_joint_value_target([0.02, 0.0])
left_gripper.go()
# left_gripper.set_joint_value_target([0.02, -0.02])
# left_gripper.go()
# left_gripper.set_joint_value_target([0.02, -0.02])
# left_gripper.go()
# left_gripper.set_joint_value_target([0.02, -0.02])
# left_gripper.go()
# rospy.sleep(2.0)
# left_gripper.set_joint_value_target([0.02, -0.02])
# left_gripper.go()
# c = left_gripper.get_current_joint_values()
# left_gripper.set_joint_value_target([0.02,-0.02])
# left_gripper.go()
# left_gripper.go([0.02,-0.02])
# rospy.sleep(2.0)
#
# left_gripper.go()
# print(a, b, c)
#
# # Plan and execute the gripper motion
# left_gripper.go(GRIPPER_NEUTRAL)
# rospy.sleep(1)
#
# # Set target joint values for the arm: joints are in the order they appear in
# # the kinematic tree.
# # # [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
# #
# # # Set the arm's goal configuration to the be the joint positions
#
# # Save this configuration for later
# # left_arm.remember_joint_values('saved_config', joint_positions)
#
# # Close the gripper as if picking something up
# # left_gripper.set_joint_value_target(GRIPPER_CLOSED)
# # left_gripper.go()
# # rospy.sleep(1)
#
# # Set the arm target to the named "straight_out" pose stored in the SRDF file
# # left_arm.set_named_target('straight_forward')
#
# # Plan and execute the motion
# # left_arm.go()
# # rospy.sleep(1)
# #
# # Set the goal configuration to the named configuration saved earlier
# # left_arm.set_named_target('saved_config')
# #
# # # Plan and execute the motion
# # left_arm.go()
# # rospy.sleep(1)
#
# # Open the gripper as if letting something go
# # left_gripper.set_joint_value_target(GRIPPER_OPEN)
# # left_gripper.go()
# # rospy.sleep(1)
# #
# # Return the arm to the named "resting" pose stored in the SRDF file
# # left_arm.set_named_target('resting')
# # left_arm.go()
# # rospy.sleep(1)
#
# # Return the gripper target to neutral position
# # left_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# # left_gripper.go()
# # rospy.sleep(1)
#
# # Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# #
# # # Exit the script
moveit_commander.os._exit(0)
for i in range(len(positions)):
positions[i] = Pose(position=Point(*positions[i]))
model = OrderedDict()
model['name'] = model_name
model['pose'] = positions
model['path'] = model_path
self.load_gazebo_sdf_models(model, 'bot_gazebo', 'world')
if __name__ == "__main__":
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_and_place')
rs = rbt_baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
pick_place = PickAndPlace(limb='left')
# pick_place.gazebo_client.initial_load_gazebo_models()
pick_place.get_scene()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# class ArmClient(object):
# def __init__(self, limb = 'left'):
# ns = 'robot/limb/' + limb + '/'
# self._client = actionlib.SimpleActionClient(
# ns + "follow_joint_trajectory",
# FollowJointTrajectoryAction,