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")