def _init_nodes(self, limb, light):
try:
self._limb = intera_interface.Limb(limb)
self._head = intera_interface.Head()
self._light = SawyerLight(light)
#self._head_display=intera_interface.HeadDisplay()
self._display = SawyerDisplay()
self._cuff = intera_interface.Cuff()
self._limits = intera_interface.JointLimits()
self._navigator = intera_interface.Navigator()
self._joint_names = self._limb.joint_names()
self._velocity_limits = self._limits.joint_velocity_limits()
self._stop_cmd = {}
for i, name in enumerate(self._joint_names):
self._stop_cmd[name] = 0.0
except:
print("Warning caught exception...")
traceback.print_exc()
pass
def __init__(self):
self.waypoints = Waypoints()
self.rs = intera_interface.RobotEnable()
self._init_state = self.rs.state().enabled
self.rs.enable()
# Set up arm, cuff and gripper
self.sawyer_arm = intera_interface.Limb('right')
self.sawyer_gripper = intera_interface.Gripper()
self.sawyer_cuff = intera_interface.Cuff(limb='right')
self.sawyer_lights = intera_interface.Lights()
# Move to default position when the ik solver is initially launched
self.sawyer_arm.move_to_neutral(timeout=10, speed=0.28)
self.sawyer_gripper.open()
self.gripper_dist = self.sawyer_gripper.get_position()
self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right'))
self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right'))
self.set_light_status('red', False)
self.set_light_status('green', True)
self.set_light_status('blue', False)
def init():
global g_limb, g_orientation_hand_down, g_position_neutral
rospy.init_node('motion')
g_limb = intera_interface.Limb('right')
print("in init")
#publisher/subscriber calls
global subscriber_voicerec, subscriber_foundobj, subscriber_objectLocation
#subscribe to speech_recognition
subscriber_voicerec = rospy.Subscriber('/speech_recognition', String,
callback_search_target)
#subscribe to foundObject
subscriber_foundobj = rospy.Subscriber('/foundObject', String,
callback_found_obj)
#subscribe to objectLocation
subscriber_objectLocation = rospy.Subscriber('/objectLocation', Pose2D,
callback_object_location)
# This quaternion will have the hand face straight down (ideal for picking tasks)
g_orientation_hand_down = Quaternion()
g_orientation_hand_down.x = 0.704238785359
g_orientation_hand_down.y = 0.709956638597
g_orientation_hand_down.z = -0.00229009932359
g_orientation_hand_down.w = 0.00201493272073
# This is the default neutral position for the robot's hand (no guarantee this will move the joints to neutral though)
g_position_neutral = Point()
g_position_neutral.x = 0.449559195663
g_position_neutral.y = 0.16070379419
g_position_neutral.z = 0.212938808947
def __init__(self, params):
self._dyn = params.reconfig_server
self._freq, self._missed_cmds = 20.0, 5.0
# Control parameters
self.bounds = params.bound * np.ones([7])
# Create our limb instance
self._limb = intera_interface.Limb(params.get('limb', 'right'))
# Create cuff disable publisher
cuff_ns = "robot/limb/%s/supress_cuff_interaction" % params.get('limb', 'right')
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
# Verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
# PD controller gains for resets
self.P = np.array([5, 5, 10, 15, 30, 20, 25]) # P controller gains
self.K = np.array([0.7, 0.2, 1.63, 0.5, 3.5, 4.7, 6.0]) # D controller gains
self._smoother_params = params.smoother_params
self._smoother_history = np.zeros(shape=[self._smoother_params.get("history_length", 1), 7])
self.sent_torques = []
def move_position(xs, ys, zs):
limb = intera_interface.Limb('right')
kinematics = sawyer_kinematics('right')
xyz_pos = [xs, ys, zs]
x = dict(zip(limb._joint_names, kinematics.inverse_kinematics(xyz_pos)))
limb.move_to_joint_positions(x)
rospy.sleep(1.0)
def main():
angle=[0,0,0,0,0,0,0]
limb=intera_interface.Limb("right")
angles=limb.joint_angles()
for i in range(7):
angle[i]=angles['right_j'+`i`]
return angle
def reset():
limb = intera_interface.Limb('right')
default_joint_angle = {'right_j0': 0.0}
limb.move_to_joint_positions(default_joint_angle)
limb.move_to_neutral()
def __init__(self):
global TF, DIST_THRE, LINE_LENGTH, HOLD_TIME, CHECKER_CENTER_COORD, FROM_CENTER_D
self._limb = intera_interface.Limb("right")
self._head = intera_interface.Head()
self.center = np.array([])
self.target_list = []
self.draw_status_list = []
self.hold_init_time = None
self.line_init_time = None
self.current_target = np.array([])
self.current_draw_status = 0 #-1 is going up, 0 is not draw, 1 is apply force control
self.if_hold = 0 #0 is not to hold, -1 is not to hold but just get back from hold. 1 is to hold
self.line_init_pose = None
self.s = 0
self.object_2_draw = "idle"
#go to camera_pose
self.go_to_camera_or_standoff('camera')
# michael camera object to pass to bennett
self.camera = gs.Camera()
self.camera.updateBoard()
self.gameRunning = True
def run():
rospy.init_node('set_joint_torques', anonymous=True)
limb = intera_interface.Limb('right')
limb.move_to_joint_positions({
'right_j0': -0.140923828125,
'right_j1': -1.2789248046875,
'right_j2': -3.043166015625,
'right_j3': -2.139623046875,
'right_j4': -0.047607421875,
'right_j5': -0.7052822265625,
'right_j6': -1.4102060546875,
})
rospy.sleep(6)
limb.set_command_timeout(0.03)
rate = rospy.Rate(33)
while True:
limb.set_joint_torques({
'right_j0': np.random.uniform(-3, 3),
'right_j1': np.random.uniform(-10, 10),
'right_j2': np.random.uniform(-5, 5),
'right_j3': np.random.uniform(-5, 5),
'right_j4': np.random.uniform(-2.25, 2.25),
'right_j5': np.random.uniform(-2.25, 2.25),
'right_j6': np.random.uniform(-2.25, 2.25)
})
rate.sleep()
def move(self, jointpos):
self.head_display.display_image("/home/microshak/Pictures/Moving.png",
False, 1.0)
print "MOVING!!!!!!!!!!!!!!!!!"
position = ast.literal_eval(json.dumps(jointpos['Cartisian']))
print(position)
p = position["position"]
o = position["orientation"]
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = o["w"]
pose_target.orientation.x = o["x"]
pose_target.orientation.y = o["y"]
pose_target.orientation.z = o["z"]
pose_target.position.x = p["x"]
pose_target.position.y = p["y"]
pose_target.position.z = p["z"]
print pose_target
group = moveit_commander.MoveGroupCommander("right_arm")
limb = intera_interface.Limb("right")
limb.set_joint_position_speed(.1)
# limb.set_joint_position_speed(.1)
group.set_pose_target(pose_target)
# group.set_joint_value_target(pose_target)
plan2 = group.plan()
group.go(wait=True)
'''
def angle_action_server():
rospy.init_node('angle_action_server', anonymous=True)
global arm
arm = ii.Limb('right')
arm.set_joint_position_speed(ros_config.JOINT_POSITION_SPEED)
s = rospy.Service('angle_action', angle_action, execute_action)
rospy.spin()
def start_robot(self):
self.rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self.rs.enable()
self.limb = intera_interface.Limb('right')
self._sticks.set_limb(self.limb)
rospy.Subscriber("/robot/limb/right/endpoint_state", EndpointState,
self._cb_endpoint_received)
def __init__(self, reconfig_server, limb="right"):
self._dyn = reconfig_server
# create our limb instance
self._limb = intera_interface.Limb(limb)
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._performedMotion = False
self.marker = Marker()
self.tf_buffer = tf2_ros.Buffer(
rospy.Duration(1200.0)) #tf buffer length
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
print("Running. Ctrl-c to quit")
self.polishTouchPointSub = rospy.Subscriber("/polishTouchPose",
PoseStamped,
self.pose_callback)
self.markerPub = rospy.Publisher("/polishTouchGotoPose", Marker)
self.flagPub = rospy.Publisher("/flag_topic", String)
rospy.on_shutdown(self.clean_shutdown)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
self.flagPub.publish(str(self._performedMotion))
if self._performedMotion:
self.markerPub.publish(self.marker)
rate.sleep()
def __init__(self,
limb="right",
hover_distance=0.16,
tip_name="right_hand"):
self._limb = intera_interface.Limb(limb)
self._gripper = intera_interface.Gripper()
self._limb_name = limb
self._process = bool
self._hover_distance = hover_distance
self._tip_name = tip_name
self._starting_pose = Pose()
self._gripper.set_cmd_velocity(0.16)
self.speed_ratio = 0.3
self.accel_ratio = 0.4
self.linear_speed = 0.3
self.linear_accel = 0.4
self._Sequence = server.Sequence()
self._camera_check = self._Sequence.poses['Camera_check']
self._zero_pose = self._Sequence._zero_pose
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self):
super(MoveGroupPythonInteface, self).__init__()
## First initialize `moveit_commander`_ and a `rospy`_ node:
# joint_state_topic = ['joint_states:=/robot/joint_states']
moveit_commander.roscpp_initialize(sys.argv)
# moveit_commander.roscpp_initialize(joint_state_topic)
rospy.init_node('move_group_python_interface', anonymous=True)
self.limb = intera_interface.Limb('right')
# roscpp_initialize(sys.argv)
# self.limb.move_to_neutral()
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints.
group_name = "right_arm"
group = moveit_commander.MoveGroupCommander(group_name)
# group.set_planner_id("RRTkConfigDefault")
group.set_planner_id("RRTConnectkConfigDefault")
## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
# Misc variables
self.box_name = 'roller'
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def __init__(self):
rospy.loginfo("Creating IK Controller class")
# setup flags and useful vars
self.running_flag = False
self.step_scale = rospy.get_param("~scale", STEP_SCALE)
self.freq = rospy.get_param("~freq", FREQ)
self.arm = rospy.get_param("~arm", ARM)
self.q = np.zeros(7)
with cl.suppress_stdout_stderr():
self.urdf = URDF.from_parameter_server()
self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm)
self.goal = np.array(self.kin.forward(self.q))
self.mutex = threading.Lock()
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q_sim = np.zeros(7)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_names = self.kin.get_joint_names()
self.qdot_dict = {}
self.limb_interface = intera_interface.Limb()
self.ep = EndpointState()
# create all subscribers, publishers, and timers
self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb)
self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb)
self.endpoint_update_cb = rospy.Subscriber("robot/limb/right/endpoint_state", EndpointState, self.endpoint_upd_cb)
def __init__(self):
rospy.init_node("pick_and_place_node", log_level=rospy.DEBUG)
self._hover_distance = rospy.get_param("~hover_distance", 0.2)
self._limb_name = rospy.get_param("~limb", 'right')
self._limb = intera_interface.Limb(self._limb_name)
self._limb.set_joint_position_speed(0.3)
self._gripper = intera_interface.Gripper(self._limb_name)
if self._gripper is None:
rospy.logerr("Gripper error")
else:
rospy.logdebug("Gripper OK")
self._head = intera_interface.Head()
self._iksvc_name = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService"
rospy.wait_for_service(self._iksvc_name, 5.0)
self._iksvc = rospy.ServiceProxy(self._iksvc_name, SolvePositionIK)
# Enable the robot
_rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
_init_state = _rs.state().enabled
rospy.logdebug("Enabling robot... ")
_rs.enable()
rospy.Service("pick_and_place", PickAndPlace, self.pnp)
rospy.Service("pick_and_throw", PickAndPlace, self.pnt)
rospy.Service("move_to_position", PositionMovement,
self.move_to_position)
rospy.Service("move_to_joint", JointMovement, self.move_to_joint)
rospy.Service("move_head", HeadMovement, self.move_head)
#rospy.Service("throw", Throw, self.throw)
rospy.logdebug("PNP Ready")
def callback_func(data):
rospy.loginfo("I got the message")
try:
grip = intera_interface.Gripper('right_gripper')
except ValueError:
rospy.logerr("ERROR: Unable to detect Gripper")
return
rospy.loginfo("Gripper position is {}".format(grip.get_position()))
tmp_limb = intera_interface.Limb('right')
end_pt = tmp_limb.endpoint_pose()['position']
print("Endpoint is x: {x}\n y: {y}\n z: {z}".format(x=end_pt[0],
y=end_pt[1],
z=end_pt[2]))
# new_pt = Point(
# x = end_pt[0],
# y = end_pt[1],
# z = end_pt[2]-0.09
# )
# tmp_limb.set_joint_trajectory(["right_j6", "right_j4"], new_pt, [0.3, 0.3], [0.01, 0.01])
grip.set_position(0.01)
rospy.loginfo("Gripper position is {}".format(grip.get_position()))
rospy.signal_shutdown("Finished grip action")
def __init__(self, limb="right"):
# Constants
self._key_command_torques = "cs225a::sawyer::actuators::fgc"
self._key_joint_positions = "cs225a::sawyer::sensors::q"
self._key_joint_velocities = "cs225a::sawyer::sensors::dq"
self._key_timestamp = "cs225a::sawyer::timestamp"
self._dof = 7
self._control_rate = 2500.0 # Hz
self._timeout_missed_cmds = 20.0 # Missed cycles before triggering timeout
self._joint_names = ["{0}_j{1}".format(limb, i) for i in range(self._dof)]
# Initialize limb interface
self._limb = intera_interface.Limb(limb)
# Initialize redis instance
self._redis = redis.StrictRedis(host="127.0.0.1", port=6379, db=0)
# Reset redis q values so they aren't random
self._redis.set(self._key_command_torques, "0 0 0 0 0 0 0")
# Verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
# Get initial time
self._start_time = rospy.Time.from_sec(time.time())
def execute(self):
"""
Initializes the connection to the robot.
"""
robot_params = intera_interface.RobotParams()
valid_limbs = robot_params.get_limb_names()
if not valid_limbs:
logger.error("Cannot detect any limb parameters on this robot!")
return
rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
init_state = rs.state().enabled
def _shutdown_hook():
logger.info("Exiting the application ...")
if not init_state:
logger.info("Disabling robot ...")
rs.disable()
rospy.on_shutdown(_shutdown_hook)
logger.info("Enabling robot ...")
rs.enable()
if self.limb_name not in valid_limbs:
logger.error("{} is not a valid limb on this robot!".format(
self.limb_name))
# Initialize the limb and send it to the resting position.
self.limb = intera_interface.Limb(self.limb_name)
self.limb.set_joint_position_speed(0.35)
self.spin()
def doStuff():
#rospy.init_node('Pls_move') # Initializes a node with name Pls_move
limb = intera_interface.Limb(
'right') # Gets the right arm (not like theres another one)
angles = limb.joint_angles() # Gets the joint angles
print(angles['right_j0'])
new_right_j0 = angles['right_j0'] + 0.55
joint_dict = {
'right_j0': new_right_j0
} # Create dictionary with joint:value mapping
limb.move_to_joint_positions(
joint_dict) # Move the joints to the dictionary specific positions
new_angles = limb.joint_angles()
print(new_angles['right_j0'])
time.sleep(3.0)
joint_dict['right_j0'] -= 0.55
limb.move_to_joint_positions(joint_dict)
print(limb.joint_angles()['right_j0'])
def __init__(self):
rospy.loginfo("Creating VelocityController class")
# Create KDL model
with cl.suppress_stdout_stderr(): # Eliminates urdf tag warnings
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, "base", "right_hand")
self.names = self.kin.get_joint_names()
# Limb interface
self.arm = intera_interface.Limb("right")
self.hand = intera_interface.gripper.Gripper('right')
# Grab M0 and Blist from saywer_MR_description.py
self.M0 = s.M #Zero config of right_hand
self.Blist = s.Blist #6x7 screw axes mx of right arm
# Shared variables
self.mutex = threading.Lock()
self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q = np.zeros(7) # Joint angles
self.qdot = np.zeros(7) # Joint velocities
self.T_goal = np.array(self.kin.forward(self.q)) # Ref se3
# Subscriber
self.ref_pose_sub = rospy.Subscriber('ref_pose', Pose, self.ref_pose_cb)
self.hand.calibrate()
self.r = rospy.Rate(100)
while not rospy.is_shutdown():
self.calc_joint_vel()
self.r.sleep()
def __init__(self, reconfig_server, limb="right"):
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
# create our limb instance
self._limb = intera_interface.Limb(limb)
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
#
self.joint_torques = dict()
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def __init__(self, trajectory_planner, limb="right", hover_distance=0.08, tip_name="right_gripper_tip"):
"""
:param limb:
:param hover_distance:
:param tip_name:
"""
self.trajectory_planner = trajectory_planner
self._limb_name = limb # string
self._tip_name = tip_name # string
self._hover_distance = hover_distance # in meters
self._limb = intera_interface.Limb(limb)
if demo_constants.is_real_robot():
self._gripper =PSGGripper()
else:
self._gripper = intera_interface.Gripper()
self._robot_enable = intera_interface.RobotEnable()
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self, speed=0.28, accuracy=0.003726646, timeout=5):
self.sawyer_arm = intera_interface.Limb('right')
self.joint_speed = speed
self.accuracy = accuracy
self.timeout = timeout
self.waypoints = list()
self._is_recording_waypoints = False
self.rs = intera_interface.RobotEnable()
self._init_state = self.rs.state().enabled
self.rs.enable()
# Create Navigator
self.navigator_io = intera_interface.Navigator()
self.sawyer_lights = intera_interface.Lights()
# Create Gripper & Cuff
self.sawyer_gripper = intera_interface.Gripper()
self.sawyer_cuff = intera_interface.Cuff(limb='right')
self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback,
'{0}_button_upper'.format('right'))
self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback,
'{0}_button_lower'.format('right'))
# print(self.navigator_io.list_all_items())
self.sawyer_arm.move_to_neutral(timeout=5, speed=self.joint_speed)
self.sawyer_gripper.open()
self.set_light_status('red', False)
self.set_light_status('green', False)
self.set_light_status('blue', True)
def ik_service_client(limb = "right", tip_name = "right_gripper_tip"):
limb_mv = intera_interface.Limb(limb)
#gripper = intera_interface.Gripper()
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Add desired pose for inverse kinematics
current_pose = limb_mv.endpoint_pose()
#print current_pose
movement = [0.460, -0.563,0.35]
orientation = [0.0,1,0.0,0.0]
#gripper.close()
rospy.sleep(1.0)
[dx,dy,dz] = movement
[ox,oy,oz,ow] = orientation
dy = constrain(dy,-0.7596394482267009,0.7596394482267009)
dz = constrain(dz, 0.02, 1)
# up position [0.45,-0.453,0.6] 0.11 for pick up location
#poses = {'right': PoseStamped(header=hdr,pose=Pose(position=Point(x=0.450628752997,y=0.161615832271,z=0.217447307078,),
#orientation=Quaternion(x=0.704020578925,y=0.710172716916,z=0.00244101361829,w=0.00194372088834,),),),} neutral pose
#x= 0.5,y = 0.5, z= 0.5,w= 0.5 for enpoint facing forward (orientation)
#table side parametres x=0.6529605227057904, y= +-0.7596394482267009, z=0.10958623747123714)
#table straight parametres x=0.99, y=0.0, z=0.1)
poses = {
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x= dx,
y= dy,
z= dz,
),
orientation=Quaternion(
x= ox,
y= oy,
z= oz,
w= ow,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_gripper_tip') # right_hand, right_wrist, right_hand_camera
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
def __init__(self,
limb,
reconfig_server,
rate=100.0,
mode='position_w_id',
interpolation='minjerk'):
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 = intera_interface.Limb(limb, synchronous_pub=True)
self._enable = intera_interface.RobotEnable()
self._name = limb
self._interpolation = interpolation
self._cuff = intera_interface.Cuff(limb=limb)
# 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
# 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] = intera_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)
def __init__(self, filename, rate, side="right"):
"""
Records joint data to a file at a specified rate.
"""
self.gripper_name = '_'.join([side, 'gripper'])
self._filename = filename
self._raw_rate = rate
self._rate = rospy.Rate(rate)
self._start_time = rospy.get_time()
self._done = False
self._limb_right = intera_interface.Limb(side)
try:
self._gripper = intera_interface.Gripper(side)
rospy.loginfo("Electric gripper detected.")
except Exception as e:
self._gripper = None
rospy.loginfo("No electric gripper detected.")
# Verify Gripper Have No Errors and are Calibrated
if self._gripper:
if self._gripper.has_error():
self._gripper.reboot()
if not self._gripper.is_calibrated():
self._gripper.calibrate()
self._cuff = intera_interface.Cuff(side)
def init():
global g_limb, g_orientation, g_orientation2, g_position_neutral
rospy.init_node('cairo_sawyer_ik_example')
g_limb = intera_interface.Limb('right')
# This quaternion will have the hand face straight down (ideal for picking tasks)
""" g_orientation_hand_down = Quaternion()
g_orientation_hand_down.x = 0.704238785359
g_orientation_hand_down.y =0.709956638597
g_orientation_hand_down.z = -0.00229009932359
g_orientation_hand_down.w = 0.00201493272073 """
# Ros quaternion info - http://wiki.ros.org/tf2/Tutorials/Quaternions
#FIRST ROTATION
q_orig = quaternion_from_euler(0, 0, 0) # Set origin
q_rot1 = quaternion_from_euler(0, pi/2, 0) # Rotate 90 degrees around y axis
q_rot2 = quaternion_from_euler(0, 0, pi/4) # Rotate 45 degrees around z axis
quat_tf = quaternion_multiply(q_rot2, quaternion_multiply(q_rot1, q_orig)) # multiply rotations in order
g_orientation = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3]) # Create Quaternion object ROS can read
#SECOND ROTATION
q_rot3 = quaternion_from_euler(0, 0,0.3) # rotate 0.3 around z axis
quat_tf2 = quaternion_multiply(q_rot3, quat_tf) # Multiply rotation times position quat_tf to get quat_tf2
g_orientation2 = Quaternion(quat_tf2[0], quat_tf2[1], quat_tf2[2], quat_tf2[3]) # Create Quaternion object ROS ccan read
# This is the default neutral position for the robot's hand (no guarantee this will move the joints to neutral though)
g_position_neutral = Point()
g_position_neutral.x = 0.449559195663
g_position_neutral.y = 0.16070379419
g_position_neutral.z = 0.212938808947
def move_joints(side):
limb = intera_interface.Limb(side)
joints = limb.joint_names()
# Publish end-effector torques to a topic
pub_torque = rospy.Publisher('endpoint_torque', Wrench, queue_size = 10)
action_count = 0; i = 0;
while not rospy.is_shutdown():
force = limb.endpoint_effort()['force']
torque = limb.endpoint_effort()['torque']
w = Wrench(Vector3(force.x, force.y, force.z), Vector3(torque.x, torque.y, torque.z));
pub_torque.publish(w);
action_count += 1;
if action_count % 10000 != 0:
continue
else:
action_count = 0
# Move joints according to specified locations
print(force)
i += 1;
if (i >= len(default_positions)):
i = 0;
raw_input("Press Enter to continue..." + str(i))
limb.set_joint_positions(default_positions[i]);
diff = sum([(limb.joint_angle(key) - default_positions[i][key])** 2 for key in default_positions[i]])
while (diff > 0.0001):
limb.set_joint_positions(default_positions[i])
diff = sum([(limb.joint_angle(key) - default_positions[i][key])** 2 for key in default_positions[i]])
time.sleep(0.005)
