def inicio():
running_status = True
limb = Limb()
state = "false"
while not rospy.is_shutdown():
if running_status:
j_p=limb.joint_angles()
state = "true"
data = {'head_pan':round(180*j_p['right_j0']/3.14,1),
'right_j0':round(180*j_p['right_j0']/3.14,1),
'right_j1':round(180*j_p['right_j1']/3.14,1),
'right_j2':round(180*j_p['right_j2']/3.14,1),
'right_j3':round(180*j_p['right_j3']/3.14,1),
'right_j4':round(180*j_p['right_j4']/3.14,1),
'right_j5':round(180*j_p['right_j5']/3.14,1),
'right_j6':round(180*j_p['right_j6']/3.14,1) }
success = device_client.publishEvent(
"joint_angle",
"json",
{'d': data},
qos=0,
on_publish=my_on_publish_callback)
#print data
time.sleep(0.3)
if not success:
print("Not connected to WatsonIoTP")
def goToPose(self):
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=0.1,
max_linear_accel=0.1,
max_rotational_speed=0.1,
max_rotational_accel=0.1,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
waypoint.set_joint_angles(self.ikResults.values(), "left_hand",
joint_names)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=30.0)
if result is None:
rospy.logerr('Trajectory Failed')
return
if result.result:
rospy.loginfo('Trajectory Success')
else:
rospy.logerr('Trajectory failed with error %s', result.errorId)
def main():
rospy.init_node("get_arm_pose")
arm = Limb()
tf_listener = tf.TransformListener()
armpose = arm.endpoint_pose()
quat_arr = armpose['orientation']
print("position is {}".format(armpose['position']))
print("\n")
print("orientation is {}".format(quat_arr))
# euler_angles = euler_from_quaternion(quat_arr, 'sxyz')
# print("The euler angles are {}".format(euler_angles))
#joints = arm.joint_angles()
#print(joints)
# rate = rospy.Rate(10.0)
# while not rospy.is_shutdown():
# try:
# # (controller_pos, controller_quat) = listener.lookupTransform('world', 'controller', rospy.Time(0))
# (pos, quat) = tf_listener.lookupTransform('world', 'reference/right_hand', rospy.Time(0))
# print("right hand orientation is {}".format(euler_from_quaternion(quat, 'sxyz')))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# continue
# rate.sleep()
rospy.spin()
def main():
rospy.init_node('testing_node')
limb = Limb('right')
print('yay')
# r = rospy.Rate(1000)
# limb.set_command_timeout(.1)
wrench = limb.endpoint_effort()
force = wrench['force']
mag = force_mag(force)
force_2d_dir = normalize(
proj(force, normalize([1, 1, 0])) * normalize([force[0], force[1], 0]))
rotation_matrix = np.matrix([[np.cos(np.pi / 2), -np.sin(np.pi / 2), 0],
[np.sin(np.pi / 2),
np.cos(np.pi / 2), 0], [0, 0, 1]])
force_base_frame = rotation_matrix * np.reshape(force_2d_dir, (3, 1))
force_base_frame[0] = -force_base_frame[0]
force_base_frame = np.array(force_base_frame).flatten()
print("force_base_frame: ", force_base_frame)
print("force_base_frame: ", type(force_base_frame))
# print(force_2d_dir)
cur_pos = limb.endpoint_pose().get('position')
print("force: ", force)
print("force 2d dir", type(force_2d_dir))
def main():
print('sup')
rospy.init_node('sawyer_kinematics')
print('init')
# rospy.sleep(10)
# init node????
# step 1 determine initial point
# step 2 determine desired endpoint
# in loop now
# step 3 measure force and position
# step 4 determine force we need to apply to push towards desired endpoint
# step 5 convert force to joint torques with Jacobian
# step 6 command joint torques
# end of loop
r = rospy.Rate(200)
limb = Limb()
kin = sawyer_kinematics('right')
#step 1
pose = limb.endpoint_pose()
point = pose.get('position')
quaternion = pose.get('orientation') # both are 3x
#step 2
path = np.array([.1, 0, 0])
des_point = point + path
des_quaternion = quaternion
path_dir = normalize(path)
#step 3
#stiffness
alpha = 1 #multiplier
while not rospy.is_shutdown() and error > tol:
#step 3
force = get_end_force(limb)
# print("force is:", force)
f_mag = force_mag(force)
f_dir = force_dir(force)
#step 4
f_right = proj(path, force) * path_dir
f_wrong = force - f_right
force_apply = f_right * alpha - f_wrong * K
force_apply = np.hstack((force_apply, np.array([0, 0, 0])))
# print('force_apply is:', force_apply)
#step 5
Jt = kin.jacobian_transpose()
joint_torques = np.dot(Jt, force_apply) #make sure right dims
joint_torques = np.asarray(joint_torques)
joint_torques = np.array([[0, 0, 0, 0, 0, 0, 0]])
# print('set joints as:', joint_torques[0])
set_torques = joint_array_to_dict(joint_torques[0], limb)
print(set_torques)
print()
#step 6
# limb.set_joint_torques(set_torques)
cur_point = limb.endpoint_pose().get('position')
error = np.linalg.norm(des_point - cur_point)
r.sleep()
def start_position(self):
"""
Start the Sawyer to the initial position
"""
joint_command = {}
right = Limb('right')
joint_command['right_j0'] = -0.20
joint_command['right_j1'] = -0.58
joint_command['right_j2'] = 0.13
joint_command['right_j3'] = 1.36
joint_command['right_j4'] = -0.2
joint_command['right_j5'] = 2.24
joint_command['right_j6'] = 1.80
# joint_command['right_j0'] = -0.11
# joint_command['right_j1'] = 0.58
# joint_command['right_j2'] = -1.83
# joint_command['right_j3'] = 1.50
# joint_command['right_j4'] = 1.64
# joint_command['right_j5'] = 2.94
# joint_command['right_j6'] = -3.18
# joint_command['right_j0'] = -0.013
# joint_command['right_j1'] = 0.88
# joint_command['right_j2'] = -0.045
# joint_command['right_j3'] = -2.60
# joint_command['right_j4'] = -2.98
# joint_command['right_j5'] = -2.98
# joint_command['right_j6'] = -4.64
try:
print("Initializing starting position....")
right.move_to_joint_positions(joint_command)
except Exception as e:
print(e)
def attach_springs(self):
"""
Switches to joint torque mode and attached joint springs to current
joint positions.
"""
# record initial joint angles
self._start_angles = self._limb.joint_angles()
# set control rate
control_rate = rospy.Rate(self._rate)
# for safety purposes, set the control rate command timeout.
# if the specified number of command cycles are missed, the robot
# will timeout and return to Position Control Mode
self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
limb = Limb()
kin = sawyer_kinematics('right')
pose = limb.endpoint_pose()
point = pose.get('position')
quaternion = pose.get('orientation')
# loop at specified rate commanding new joint torques
while not rospy.is_shutdown():
if not self._rs.state().enabled:
rospy.logerr("Joint torque example failed to meet "
"specified control rate timeout.")
break
self._update_forces(limb, kin, point, quaternion)
control_rate.sleep()
def __init__(self, path):
rospy.init_node('sawyer_arm_cntrl')
self.load_inits(path+'/pg_init.yaml')
self.limb = Limb()
self.all_jnts = copy.copy(self.limb.joint_names())
self.limb.set_joint_position_speed(0.2)
def joint_angles(args):
# arg_fmt = argparse.RawDescriptionHelpFormatter
# parser = argparse.ArgumentParser(formatter_class=arg_fmt,
# description=main.__doc__)
# parser.add_argument(
# "-q", "--joint_angles", type=float,
# nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
# help="A list of joint angles, one for each of the 7 joints, J0...J6")
# parser.add_argument(
# "-s", "--speed_ratio", type=float, default=0.5,
# help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
# parser.add_argument(
# "-a", "--accel_ratio", type=float, default=0.5,
# help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
# parser.add_argument(
# "--timeout", type=float, default=None,
# help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
try:
#rospy.init_node('go_to_joint_angles_py')
limb = Limb()
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(
max_joint_speed_ratio=args.speed_ratio,
max_joint_accel=args.accel_ratio)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(args.joint_angles) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles=args.joint_angles)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=args.timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
return True
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def __init__(self):
self.Blist = sw.Blist
self.M = sw.M
self.Slist = mr.Adjoint(self.M).dot(self.Blist)
self.eomg = 0.01 # positive tolerance on end-effector orientation error
self.ev = 0.001 # positive tolerance on end-effector position error
self.arm = None
self.listener = None
self.home_config = {
'right_j6': -1.3186796875,
'right_j5': 0.5414912109375,
'right_j4': 2.9682451171875,
'right_j3': 1.7662939453125,
'right_j2': -3.0350302734375,
'right_j1': 1.1202939453125,
'right_j0': -0.0001572265625
}
self.tracker_position = None
self.tracker_pos_mag = None
self.arm_position = None
self.sword_position = None
self.attack_position = None
self.tag_position = None
self.disp_mag = None
self.attack_disp_mag = None
self.fence_region = 1.50
self.fence_speed = 0.50
self.sword_zoffset = 0.25
self.end_effector_directions = [-1.0, 1.0]
self.tracker_twist = 50
self.end_effector_vel = np.zeros(6)
# velocities need to be passed in as a dictionary
self.joint_vels_dict = {}
# set the end effector twist
self.sword_twist = Twist()
self.arm_twist = Twist()
self.no_twist = Twist()
self.no_twist.linear.x = 0
self.no_twist.linear.y = 0
self.no_twist.linear.z = 0
self.no_twist.angular.x = 0
self.no_twist.angular.y = 0
self.no_twist.angular.z = 0
self.arm = Limb()
print("going to home configuration")
self.arm.set_joint_positions(self.home_config)
self.clash_pub = rospy.Publisher('clash', Float64, queue_size=1)
self.twist_sub = rospy.Subscriber('/vive/twist1',
TwistStamped,
self.twist_callback,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.rate = rospy.Rate(10.0)
def __init__(self):
self._done = False
self._head = Head()
self._limb=Limb()
self._robot_state = RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
self._robot_state.enable()
self._display = HeadDisplay()
face_forward_service = rospy.Service('head/face_forward', FaceForward, self.face_forward)
rotate_head_service= rospy.Service('head/pan_to_angle', PanToAngle, self.pan_to_angle)
def move(self, point_list, wait = True, MAX_LIN_SPD=7.0, MAX_LIN_ACCL=1.5): # one point = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg]
try:
limb = Limb() # point_list = [pointA, pointB, pointC, ...]
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
except:
print("There may have been an error while exiting")
if self.STOP:
traj.stop_trajectory()
return True
wpt_opts = MotionWaypointOptions(max_linear_speed=MAX_LIN_SPD, max_linear_accel=MAX_LIN_ACCL, corner_distance=0.002)
for point in point_list:
q_base = quaternion_from_euler(0, 0, math.pi/2)
#q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5]))
q_rot = quaternion_from_euler(point[3], point[4], point[5])
q = quaternion_multiply(q_rot, q_base)
newPose = PoseStamped()
newPose.header = Header(stamp=rospy.Time.now(), frame_id='base')
newPose.pose.position.x = point[0] + 0.65
newPose.pose.position.y = point[1] + 0.0
newPose.pose.position.z = point[2] + 0.4
newPose.pose.orientation.x = q[0]
newPose.pose.orientation.y = q[1]
newPose.pose.orientation.z = q[2]
newPose.pose.orientation.w = q[3]
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles())
traj.append_waypoint(waypoint.to_msg())
if(wait):
print(" \n --- Sending trajectory and waiting for finish --- \n")
result = traj.send_trajectory(wait_for_result=wait)
if result is None:
rospy.logerr('Trajectory FAILED to send')
success = False
elif result.result:
rospy.loginfo('Motion controller successfully finished the trajcetory')
success = True
else:
rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId)
success = False
else:
print("\n --- Sending trajector w/out waiting --- \n")
traj.send_trajectory(wait_for_result=wait)
success = True
return success
def move_to_home(thetalist):
rightlimb = Limb()
waypoints = {}
waypoints['right_j0'] = thetalist[0]
waypoints['right_j1'] = thetalist[1]
waypoints['right_j2'] = thetalist[2]
waypoints['right_j3'] = thetalist[3]
waypoints['right_j4'] = thetalist[4]
waypoints['right_j5'] = thetalist[5]
waypoints['right_j6'] = thetalist[6]
waypoints['torso_t0'] = thetalist[7]
rightlimb.move_to_joint_positions(waypoints, timeout=20.0, threshold=0.05)
rospy.sleep(2.0)
def moveRoboticArm(position, orientation, linear_speed, linear_accel):
"""
Move the robot arm to the specified configuration given a positionX, positionY, positionZ, quaternion array and max linear speed.
"""
try:
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed,
max_linear_accel=linear_accel)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
endpoint_state = limb.tip_state('right_hand')
pose = endpoint_state.pose
if position is not None and len(position) == 3:
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
if orientation is not None and len(orientation) == 4:
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory()
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
print("Something went wrong")
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def __init__(self):
self._start_pose = {
'position':
Point(x=0.83580435659716,
y=0.14724066320869522,
z=0.4548728070777701),
'orientation':
Quaternion(x=0.7366681513777151,
y=-0.6758459333695754,
z=0.023326822897651703,
w=0.002858046019378824)
}
self._pixel_meter_ratio = 0.001
self._threshold = 4
self.harmonic_repeat = float(2)
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
self.compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self.right_gripper = robot_gripper.Gripper('right_gripper')
self.limb = Limb()
self._planning_scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
def __init__(self,
label=None,
joint_names=None,
trajectory_options=None,
limb=None):
"""
Creates a new motion trajectory object.
@param label: string
if label is None: use default
@param joint_names: name for each joint in the robot
if joint_names is None: use default names
@param trajectory_options: options for the trajectory
if trajectory_options is None: use default options
@param limb: Limb object
if limb is None: create a new instance of Limb
"""
# Used for sending the trajectory to the motion controller
self._client = MotionControllerActionClient()
self._traj = Trajectory()
self._limb = limb or Limb()
self.set_label(label)
self.set_joint_names(joint_names)
self.set_trajectory_options(trajectory_options)
def observe(time):
joint_command = {}
right = Limb('right')
joint_command['right_j0'] = 0.75
joint_command['right_j1'] = 0.22
joint_command['right_j2'] = -1.72
joint_command['right_j3'] = 1.17
joint_command['right_j4'] = -1.37
joint_command['right_j5'] = -2.97
joint_command['right_j6'] = -1.54
for _ in range(time):
try:
right.set_joint_positions(joint_command)
except KeyboardInterrupt:
print('done')
raw_input("Ready to find Shape. Press <Enter>")
def main():
#planner = PathPlanner("right_arm")
controller = velocityControl(Limb("right"))
#[start_x,start_y,h] = [0.49,-0.3,0.23+0.15] # ar_tag.z=-0.23, constant offset=+0.66
[start_x,start_y,h] = [0.6712,0.1505,0.23+0.213]
# [start_x,start_y,h] = [0.54, 0.30, -0.00288962+0.38]
joints_position = coorConvert.cartesian2joint(start_x,start_y,h)
positionControl.positionControl(jointCom=joints_position)
def goto_cartesian(self, x, y, z):
print("goto_cartesian called with x={}, y={}, z={}".format(x, y, z))
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
endpoint_state = limb.tip_state('right_hand')
if endpoint_state is None:
print('Endpoint state not found')
return self.MOVE_ERROR
pose = endpoint_state.pose
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.x = self.orientation_x
pose.orientation.y = self.orientation_y
pose.orientation.z = self.orientation_z
pose.orientation.w = self.orientation_w
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, 'right_hand', [])
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory()
if result is None:
print("Trajectory FAILED to send")
return self.MOVE_ERROR
if result.result:
print('Motion controller successfully finished the trajectory!')
self.cur_x = x
self.cur_y = y
self.cur_z = z
return self.MOVE_SUCCESS
else:
print('Motion controller failed to complete the trajectory %s',
result.errorId)
return self.MOVE_ERROR
def __init__(self):
self.robot_state = 0 # normal
self.breath_state =0 # false
rospy.Subscriber("cs_sawyer/head_light", UInt8, self.callback_update_breath1)
rospy.Subscriber("cs_sawyer/breath", Bool, self.callback_update_breath2)
self.rospack = rospkg.RosPack()
# Set the trajectory options
self.limb = Limb()
traj_opts = TrajectoryOptions()
traj_opts.interpolation_type = 'JOINT'
self.traj = MotionTrajectory(trajectory_options = traj_opts, limb = self.limb)
# Set the waypoint options
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.05, joint_tolerances=0.7)
#max_joint_accel=0.1)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self.limb)
# Append a waypoint at the current pose
waypoint.set_joint_angles(self.limb.joint_ordered_angles())
#self.traj.append_waypoint(waypoint.to_msg())
#self.limb.set_joint_position_speed(0.3)
with open(join(self.rospack.get_path("cs_sawyer"), "config/poses.json")) as f:
self.poses = json.load(f)
joint_angles= [self.poses["pause"][j] for j in [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']]
j1= joint_angles[1]
j2= joint_angles[2]
x=0
while x < 16*pi:
new_j1 = 0.07*sin(x)+j1
new_j2=0.09*sin(0.5*x)+j2
joint_angles[1]=new_j1
joint_angles[2]=new_j2
x=x+pi/40
waypoint.set_joint_angles(joint_angles = joint_angles)
self.traj.append_waypoint(waypoint.to_msg())
def planning():
rate = rospy.Rate(10) # 10hz
limb = Limb('right')
default_joints = {
'head_pan': -4.2551240234375,
'right_j0': -2.3731005859375,
'right_j1': -2.4028828125,
'right_j2': 1.658787109375,
'right_j3': 0.7297041015625,
'right_j4': 1.2216513671875,
'right_j5': 0.31765625,
'right_j6': -4.6892177734375,
'torso_t0': 0.0
}
# right_arm.set_joint_positions(default_joints)
limb.set_joint_positions(default_joints)
def __init__(self):
self.mover = move_to_point.MoveToPoint()
self.retrieve_height = -0.015
self._limb = Limb()
self.traj_options = TrajectoryOptions()
self.traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
self.traj = MotionTrajectory(trajectory_options=self.traj_options,
limb=self._limb)
self.wpt_options = MotionWaypointOptions(max_linear_speed=0.4,
max_linear_accel=0.4,
max_rotational_speed=1.57,
max_rotational_accel=1.57,
max_joint_speed_ratio=1.0)
self.waypoint = MotionWaypoint(options=self.wpt_options.to_msg(),
limb=self._limb)
self.joint_names = self._limb.joint_names()
endpoint_state = self._limb.tip_state('right_hand')
self.pose = endpoint_state.pose
def gripper_pose(x, y, z):
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
endpoint_state = limb.tip_state('right_hand')
if endpoint_state is None:
print('Endpoint state not found')
return False
pose = endpoint_state.pose
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, 'right_hand', [])
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory()
if result is None:
print("Trajectory FAILED to send")
return False
if result.result:
return True
else:
print('Motion controller failed to complete the trajectory %s',
result.errorId)
return False
def main():
rospy.init_node('testing_node')
limb = Limb('right')
print('yay')
r = rospy.Rate(200)
kin = sawyer_kinematics('right')
Jt = kin.jacobian_transpose()
F = np.array([0, 0, 0, 0, 0, 10])
tau = np.dot(Jt, F)
# print(tau)
# rospy.sleep(20)
# limb.set_command_timeout(.1)
# wrench = limb.endpoint_effort()
# force = wrench['force']
# print("force type: ", force)
# print(wrench)
while not rospy.is_shutdown():
# joint_torques = np.array([[0,0,0,0,0,0,0]])
Jt = kin.jacobian_transpose()
mcart = kin.cart_inertia()
f = np.array([0, 0, -9.8, 0, 0, 0])
gravity = np.dot(np.dot(Jt, mcart), f)
joint_torques = np.array([
1.5474950095623006, -16.78679653980678, -2.8277487768926406,
-0.1771867794616826, 0.1073210015442511, -0.5216893350253217,
-0.00607477942479895
])
# cur = limb.joint_efforts()
# print('set joints as:', joint_torques[0])
# set_torques = joint_array_to_dict(joint_torques[0], limb)
# print(set_torques)
# print(set_torques.get('right_j1'))
# torque = np.array([gravity.item(i) for i in range(7)])
torque = joint_array_to_dict(joint_torques, limb)
# cur['right_j1'] = 0
# cur['right_j2'] = 0
print('joint torques', torque)
limb.set_joint_torques(torque)
r.sleep()
def path_planning(position, orientation ,max_speed = 0.1, max_accel = 0.1):
"""
Plan the path from current position to the desired position
Parameters
----------------
position: list,
the desired destination
max_speed: float,
the maximum value of the speed we want
max_accel: float,
the maximum value of the accel we want
Return
----------------
waypoint: MotionWaypoint
the planned path. May only be a part of the final trajectory
"""
poses = {
'right': PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=position[0],
y=position[1],
z=position[2],
),
orientation=Quaternion(
x=orientation[0],
y=orientation[1],
z=orientation[2],
w=orientation[3],
),
),
),
}
limb = Limb()
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=max_speed,
max_joint_accel=max_accel)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
joint = ik_service_client(poses).values()[::-1] # joint angles from J0 to J6
if len(joint) != 7:
rospy.logerr('The number of joint_angles must be 7')
return None
waypoint.set_joint_angles(joint_angles = joint)
return waypoint
def go_to_angles(joint_angles_goal, speed_ratio_goal, accel_ratio_goal,
timeout_goal):
try:
#rospy.init_node('go_to_joint_angles_py') # initialze once
limb = Limb()
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(
max_joint_speed_ratio=speed_ratio_goal,
max_joint_accel=accel_ratio_goal)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(joint_angles_goal) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles=joint_angles_goal)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout_goal)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
class KBEnv(object):
def __init__(self, path):
rospy.init_node('sawyer_arm_cntrl')
self.load_inits(path+'/pg_init.yaml')
self.limb = Limb()
self.all_jnts = copy.copy(self.limb.joint_names())
self.limb.set_joint_position_speed(0.2)
def load_inits(self, path):
stream = open(path, "r")
config = yaml.load(stream)
stream.close()
# these indices and names have to be in ascending order
self.jnt_indices = config['jnt_indices']
self.cmd_names = config['cmd_names']
self.init_pos = config['initial-position']
def chng_jnt_state(self, values, mode, iterations=20):
# Command Sawyer's joints to angles or velocity
robot = URDF.from_parameter_server()
kdl_kin_r = KDLKinematics(robot, 'base',
'right_gripper_r_finger_tip')
kdl_kin_l = KDLKinematics(robot, 'base',
'right_gripper_l_finger_tip')
q_jnt_angles = np.zeros(8)
q_jnt_angles[:7] = copy.copy(values)
q_jnt_angles[7] = 0.0
pose_r = kdl_kin_r.forward(q_jnt_angles)
pose_l = kdl_kin_l.forward(q_jnt_angles)
pose = 0.5*(pose_l + pose_r)
x, y, z = transformations.translation_from_matrix(pose)[:3]
print("goal cartesian: ", x, y, z)
timeout = 30.0
if mode == 4:
positions = dict()
i = 0
for jnt_name in self.all_jnts:
positions[jnt_name] = values[i]
i += 1
self.limb.move_to_joint_positions(positions, timeout)
else:
velocities = dict()
i = 0
for name in self.cmd_names:
velocities[name] = values[i]
i += 1
for _ in range(iterations):
self.limb.set_joint_velocities(velocities)
time.sleep(0.5)
def ready(time):
joint_command = {}
right = Limb('right')
joint_command['right_j0'] = 0.5
joint_command['right_j1'] = 0
joint_command['right_j2'] = -1.4
joint_command['right_j3'] = 1
joint_command['right_j4'] = -1.7
joint_command['right_j5'] = -1
joint_command['right_j6'] = -1.7
for _ in range(time):
try:
right.set_joint_positions(joint_command)
except KeyboardInterrupt:
print('done')
# right_gripper = robot_gripper.Gripper('right_gripper')
# right_gripper.calibrate()
# rospy.sleep(2.0)
# right_gripper.close()
# rospy.sleep(1.0)
raw_input("Ready to draw. Press <Enter>")
def main():
"""
This is an example script to demonstrate the functionality of the MotionTrajectory
to read a yaml file. This script reads in the yaml file creates a trajectory object
and sends the trajectory to the robot.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-f",
"--file_name",
type=str,
required=True,
help="The name of the yaml file to ready the trajectory from")
parser.add_argument(
"--timeout",
type=float,
default=None,
help=
"Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout."
)
args = parser.parse_args(rospy.myargv()[1:])
try:
rospy.init_node('send_traj_from_file')
limb = Limb()
traj = MotionTrajectory(limb=limb)
read_result = traj.load_yaml_file(args.file_name)
if not read_result:
rospy.logerr('Trajectory not successfully read from file')
result = traj.send_trajectory(timeout=args.timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def sendIOT():
#rospy.init_node('avalos_limb_py')
while not rospy.is_shutdown():
limb = Limb()
j_p=limb.joint_angles()
j_v=limb.joint_velocities()
p_p=limb.endpoint_pose()
v_p=limb.endpoint_velocity()
if running_status:
joint_p = {'right_j0': j_p['right_j0'],
'right_j1': j_p['right_j1'],
'right_j2': j_p['right_j2'],
'right_j3': j_p['right_j3'],
'right_j4': j_p['right_j4'],
'right_j5': j_p['right_j5'],
'right_j6': j_p['right_j6']}
joint_v = {'right_j0': j_v['right_j0'],
'right_j1': j_v['right_j1'],
'right_j2': j_v['right_j2'],
'right_j3': j_v['right_j3'],
'right_j4': j_v['right_j4'],
'right_j5': j_v['right_j5'],
'right_j6': j_v['right_j6']}
success = device_client.publishEvent(
"joint_angle",
"json",
{'j_p': joint_p,'j_v': joint_v},
qos=0,
on_publish=my_on_publish_callback())
#print data
time.sleep(0.1)
if not success:
print("Not connected to WatsonIoTP")
