def get_ik(position): """Get the inverse kinematics Get the inverse kinematics of the UR5 robot using track_IK package giving a desired intial position Arguments: position {list} -- A position representing x, y and z Returns: sol {list} -- Joint angles or None if track_ik is not able to find a valid solution """ camera_support_angle_offset = 0.0 q = quaternion_from_euler(0.0, -3.14 + camera_support_angle_offset, 0.0) # Joint order: # ('shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'grasping_link') ik_solver = IK("base_link", "grasping_link", solve_type="Distance") sol = ik_solver.get_ik([ 0.2201039360819781, -1.573845095552878, -1.521853400505349, -1.6151347051274518, 1.5704492904506875, 0.0 ], position[0], position[1], position[2], q[0], q[1], q[2], q[3]) if sol is not None: sol = list(sol) sol[-1] = 0.0 else: print("IK didn't return any solution") return sol
class IKSolver(): def __init__(self): self.ik_solver = IK("world", "link4", timeout=0.1, solve_type="Distance") self.start_state = [0.0] * (self.ik_solver.number_of_joints - 1) self.bx = self.by = self.bz = 0.001 self.brx = self.bry = self.brz = 0.1 self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw = 0, 0, 0, 0, 0, 0, 1 rospy.init_node("ik_solver") self.ik_solution_pub = rospy.Publisher("/arm/move_jp", JointState, queue_size=10) self.position_for_controller = JointState() self.ROS_main_loop() # lb, up = self.ik_solver.get_joint_limits() def ik_find_solution(self, x, y, z, qx, qy, qz, qw): if self.x != x or self.y != y or self.z != z or self.qx != qx or self.qy != qy or self.qz != qz or self.qw != qw: self.sol = self.ik_solver.get_ik(self.curent_js.position, self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw, self.bx, self.by, self.bz, self.brx, self.bry, self.brz) self.x = x self.y = y self.z = z self.qx = qx self.qy = qy self.qz = qz self.qw = qw if self.sol != None: self.sol = [round(i, 2) for i in self.sol] print(self.sol) self.position_for_controller.position = self.sol self.position_for_controller.position.append(0.0) self.ik_solution_pub.publish(self.position_for_controller) else: print("No solution") def ik_callback(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z qx = pose.orientation.x qy = pose.orientation.y qz = pose.orientation.z qw = pose.orientation.w print(pose) self.ik_find_solution(x, y, z, qx, qy, qz, qw) def js_cb(self, js): self.curent_js.position = js.position[:-1] def ROS_main_loop(self): self.curent_js = JointState() rospy.Subscriber("/l_cont_pose", Pose, self.ik_callback) rospy.Subscriber("/arm/measured_js", JointState, self.js_cb) rospy.loginfo("Init done. IK plugin is working") rospy.spin()
def trac_ik_solve(limb, ps): print 'publish', ps target_topic.publish(ps) local_base_frame = limb + "_arm_mount" ik_solver = IK(local_base_frame, limb + "_wrist", urdf_string=urdf_str) seed_state = [0.0] * ik_solver.number_of_joints # canonical pose in local_base_frame #hdr = Header(stamp=rospy.Time.now(), frame_id=from_frame) #ps = PoseStamped( # header=hdr, # pose=pose, # ) p = translate_frame(ps, local_base_frame) #print 'translated frame',p soln = ik_solver.get_ik( seed_state, p.pose.position.x, p.pose.position.y, p.pose.position.z, # X, Y, Z p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w, # QX, QY, QZ, QW 0.01, 0.01, 0.01, 0.1, 0.1, 0.1, ) print 'trac soln', soln return soln
class GraspKDL(): ''' Grasp KDL class. ''' def __init__(self): # rospy.init_node('grasp_kdl') self.robot = URDF.from_parameter_server() base_link = 'world' end_link = 'palm_link' self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.ik_solver = IK(base_link, end_link) self.seed_state = [0.0] * self.ik_solver.number_of_joints def forward(self, q): pose = self.kdl_kin.forward(q) return pose def jacobian(self, q): J = self.kdl_kin.jacobian(q) return J def inverse(self, pose): ik_js = self.ik_solver.get_ik(self.seed_state, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) if ik_js is None: rospy.logerr('No IK solution for grasp planning!') return None return ik_js
class RobotKinematics: """Calculate direct and inverse kinematics through different methods""" def __init__(self, from_rosparam=False): if from_rosparam: try: urdf = rospy.get_param('/robot_description') except: urdf = open("modello.urdf").read() else: urdf = open("modello.urdf").read() urdf = urdf.replace('encoding="utf-8"', '').replace('encoding=\'utf-8\'', '') self.chain = kp.build_chain_from_urdf(urdf) self.njoints = len( self.chain.get_joint_parameter_names() ) self.end_link = "end_effector" # "link{}".format(self.njoints) self.ik_solver = IK("link0", self.end_link, urdf_string=urdf) def calculate_direct_kinematic(self, joint_angles): assert len(joint_angles) == self.njoints joint_state = {} for i in range(0, len(joint_angles)): joint_state["joint{}".format(i)] = joint_angles[i] all_link_positions = self.chain.forward_kinematics(joint_state) return all_link_positions[self.end_link].pos def calculate_inverse_kinematics_ik(self, x, y, z): initial_state = np.array(self.njoints * [0.0]).astype(float) return self.ik_solver.get_ik(initial_state, x, y, z, 0, 0, 0, 1.0, 1e-8, 1e-8, 1e-8) def calculate_inverse_kinematics_optimization(self, x, y, z): goal_position = np.array([x, y, z]) print("Optimize ", goal_position) def distance_from_goal(joint_angles): current_position = self.calculate_direct_kinematic(joint_angles) return np.linalg.norm(goal_position - current_position) JOINT_LIMIT = (np.pi - 0.0000001)/4.0 N_JOINTS = self.njoints bounds = ( (-JOINT_LIMIT, JOINT_LIMIT), ) * N_JOINTS initial_state = np.array(N_JOINTS * [0.0]).astype(float) print("From initial state ", initial_state) solution = minimize(distance_from_goal, initial_state, method='SLSQP', bounds=bounds) return solution.x def get_joints_number(self): return self.njoints
def trac_ik_solve(limb, ps): try: arm = baxter_interface.Limb(limb) state = arm.joint_angles() print 'solve current:', arm.joint_angles() jointnames = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2'] command = [] for i in range(0, len(jointnames)): key = limb + "_" + jointnames[i] command.append(state[key]) print 'candidate seed', command local_base_frame = limb + "_arm_mount" ik_solver = IK( local_base_frame, limb + "_wrist", #limb+"_gripper", urdf_string=urdf_str) #seed_state = [0.0] * ik_solver.number_of_joints seed_state = command # canonical pose in local_base_frame #hdr = Header(stamp=rospy.Time.now(), frame_id=from_frame) #ps = PoseStamped( # header=hdr, # pose=pose, # ) #gripper_ps = translate_in_frame(ps,'right_wrist',0,0,0) #gripper_ps = translate_pose_in_own_frame(ps,'gripper_target',0.015,-0.02,-0.2) #gripper_ps = translate_pose_in_own_frame(ps,'gripper_target',0,0,0.05) p = translate_frame(ps, local_base_frame) #print 'translated frame',p soln = ik_solver.get_ik( seed_state, p.pose.position.x, p.pose.position.y, p.pose.position.z, # X, Y, Z p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w, # QX, QY, QZ, QW #0.01,0.01,0.01, 0.1, 0.1, 0.1, 0.3, 0.3, 0.3, ) print 'trac soln', soln return soln except: return None
def ik_solver(X, Y, Z, QX, QY, QZ, QW): urdf_str = rospy.get_param('/robot_description') #print urdf_str ik_sol = IK("panda_link0","panda_link7",urdf_string=urdf_str) #print ik_sol.link_names global position seed_state = position[2:9] lower_bound, upper_bound = ik_sol.get_joint_limits() #print upper_bound #print lower_bound ik_sol.set_joint_limits(lower_bound, upper_bound) return ik_sol.get_ik(seed_state, X, Y, Z, # X, Y, Z QX, QY, QZ, QW) # QX, QY, QZ, QW
class ConbeIK(): def __init__(self, urdf_param, LorR, pos_bound, ori_bound): # Get URDF from parameter server # self._urdf_str = rospy.get_param('/LArm/robot_description') self._urdf_str = rospy.get_param(urdf_param) # Create IK instance self._ik_solver = IK(LorR + "link0", LorR + "EEFlink", urdf_string=self._urdf_str) # set the Dof self._seed_state = [0.0] * self._ik_solver.number_of_joints #set the lower&upper bound self._lower_bound, self._upper_bound = self._ik_solver.get_joint_limits( ) #can modify the joint limits # self._ik_solver.set_joint_limits([0.0]* self._ik_solver.number_of_joints, upper_bound) self._pos_bound = pos_bound self._ori_bound = ori_bound def check_setting(self): # check the info of model print('ik_solver.base_link : ', self._ik_solver.base_link) print('ik_solver.tip_link : ', self._ik_solver.tip_link) print('ik_solver.joint_names: ', self._ik_solver.joint_names) print('lower_bound : ', self._lower_bound) print('upper_bound : ', self._upper_bound) def calculate(self, pos, ori): #calculate IK result = self._ik_solver.get_ik( self._seed_state, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3], self._pos_bound[0], self._pos_bound[1], self._pos_bound[2], # X, Y, Z bounds self._ori_bound[0], self._ori_bound[1], self._ori_bound[2]) # Rotation X, Y, Z bounds return result
class RobotKinematics: def __init__(self, urdf_str): self.urdf_tree = URDF.from_xml_string(urdf_str) base_link = "link0" nlinks = len(self.urdf_tree.link_map.keys()) end_link = "link{}".format(nlinks - 1) print("Link chain goes from {} to {}".format(base_link, end_link)) self.kdl_kin = KDLKinematics(self.urdf_tree, base_link, end_link) self.ik_solver = IK(base_link, end_link, urdf_string=urdf_str) def calculate_inverse_kinematics(self, joint_state, x, y, z, rx=0, ry=0, rz=0): return self.ik_solver.get_ik(joint_state, x, y, z, rx, ry, rz, 1.0, 1e-8, 1e-8, 1e-8) def calculate_inverse_kinematics_rl(self): # Deep Deterministic Policy gradients # https://blog.floydhub.com/robotic-arm-control-deep-reinforcement-learning/ pass def calculate_direct_kinematic(self, joint_state): forward_kin_matrix = self.kdl_kin.forward(joint_state) x = forward_kin_matrix[0, -1] y = forward_kin_matrix[1, -1] z = forward_kin_matrix[2, -1] return {"x": x, "y": y, "z": z} def get_joint_names(self): # return self.ik_solver.joint_names return self.urdf_tree.joint_map.keys() def get_link_names(self): # return self.ik_solver.link_names return self.urdf_tree.link_map.keys()
def move_to_pose(limb, pose): pos, rot = pose #trac_ik kinematics intializing ik_solver = IK("base", "stp_021808TP00080_tip") current_joint_angles = limb.joint_angles() sorted_angles = sorted(current_joint_angles.items(), key=lambda kv: (kv[0], kv[1])) seed_state = [0] * ik_solver.number_of_joints for i in range(len(sorted_angles)): seed_state[i] = sorted_angles[i][1] kin = ik_solver.get_ik(seed_state, pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]) if kin == None: return None go_to_pose = dict(zip(limb.joint_names(), kin)) limb.move_to_joint_positions(go_to_pose)
class PathPlanner(): """ This class should contain all path planning functionality. The goal is that no ROS should be needed to interface with this node All ROS should be self-contained here """ def __init__(self): """ Shouldn't need inputs. Stores: 2 move groups: arm and hand - moveit commander interface planning scene interface and publisher robot commander IK solver (using trac_ik) FK solver (moveit service) """ rospy.loginfo("To stop project CTRL + C") rospy.on_shutdown(self.shutdown) # Initialize moveit_commander # I honestly don't think it actually uses the sys.argv argument, # but I need to do a little more digging first. It parses the input # for arguments containing "__name:=" which it passes to another initializer # I need to dig further to see what the name parameter actually changes moveit_commander.roscpp_initialize(sys.argv) # Instatiate the move group self.group = moveit_commander.MoveGroupCommander('arm') self.group.set_planning_time(5) self.group.set_workspace([-3, -3, -3, 3, 3, 3]) # This publishes trajectories to RVIZ for visualization # Need to figure out how to get rid of the visualization as well # Also how to use this to publish individual poses self.display_planned_path_publisher = rospy.Publisher( 'arm/display_planned_path', DisplayTrajectory, queue_size=10) # This publishes updates to the planning scene self.planning_scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) # Planning scene self.scene = moveit_commander.PlanningSceneInterface() # RobotCommander self.robot = moveit_commander.RobotCommander() # IK Solver with trac_ik # NOTE: Get this from the MoveGroup so it's adaptable to other robots self.ik_solver = IK('root', 'm1n6s300_end_effector') self.ik_default_seed = [0.0] * self.ik_solver.number_of_joints # FK Solver rospy.wait_for_service('/compute_fk') self.fk_solver = rospy.ServiceProxy('/compute_fk', GetPositionFK) rospy.sleep(2) # Rate is 10 Hz. Is this a good rate? rate = rospy.Rate(10) def shutdown(self): """ Things to do when the node is shut down (currently does nothing besides printing a message, should it do anything?) """ rospy.loginfo("Stopping project") rospy.sleep(1) def plan_to_config(self, end_state): """ Plans to a configuration using MoveIt. Does not deal with finger positions (these stay static) ------- end_state: list of joint angles, discluding fingers (length 6) ------- returns: moveit path object (array of JointStates or RobotStates I believe) """ joint_state = JointState() joint_state.header.stamp = rospy.Time.now() # the robot has a dumb base_link joint that we don't want joint_state.name = self.group.get_joints()[1:-1] print joint_state.name joint_state.position = end_state self.group.set_start_state_to_current_state() try: self.group.set_joint_value_target(joint_state) except moveit_commander.MoveItCommanderException: pass # Plan the path plan = self.group.plan() return plan def plan_to_pose(self, end_pose, seed=None): """ Plans to a pose by using inverse kinematics. ------- end_pose: rigid_transform object from autolab? (or should I write my own? Or allow different inputs?) seed: an optional input to the IK solver. It's a configuration (len 6 array) from which the solver will start. (trac_ik uses an augmented newton's method) If it's None, it'll use the current pose as the seed ------- returns: moveit path object """ raiseNotDefined() def move_home(self): """ Plans from the current state to the home position ------- returns: moveit path object """ self.group.set_start_state_to_current_state() self.group.set_named_target('Home') self.group.set_workspace([-3, -3, -3, 3, 3, 3]) plan = self.group.plan() return plan def execute_path(self, path, wait_bool=True): """ Executes an input path in the real world / gazebo / rviz ------ path: a moveit path object wait_bool: whether the node pauses until the robot reaches its destination """ self.group.execute(path, wait=wait_bool) def get_ik(self, pose, seed=None, xyz_bounds=None, rpy_bounds=None): """ Uses trac_ik to get a joint configuration from an input pose Does not include collision checking ------ pose: rigid transform from autolab? See plan_to_pose seed: list of size 6. start value for the IK. See plan_to_pose. If None, will use current pose xyz_bounds: list of size 3. xyz bounds of the end effector in meters. This allows an approximate ik solution. Default is None rpy_bounds: a list of size 3. roll pitch yaw bounds of the end effector in radians. This allows an approximate ik solution. Default is None ------ returns: joint states, a list of size 6 (or None for failure) """ if seed_state is None: seed_state = self.ik_default_seed if pose.header.frame_id != self.ik_solver.base_link: raise ValueError("Frame ID is not the root link") position = pose.pose.position orientation = pose.pose.orientation print seed_state, position.x, position.y, \ position.z, orientation.x, orientation.y, \ orientation.z, orientation.w \ if xyz_bounds is None or rpy_bounds is None: state = self.ik_solver.get_ik(seed_state, position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w) else: state = self.ik_solver.get_ik( seed_state, position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w, xyz_bounds[0], xyz_bounds[1], xyz_bounds[2], rpy_bounds[0], rpy_bounds[1], rpy_bounds[2]) return state def get_fk(self, joints): """ Uses the MoveIt service to get the end effector pose from joints Note that this is the pose of the beginning of the hand, not the hand itself ------ joints: a list of size 6. Joint states for the calculation ------ returns: a rigid transform from autolab? See plan_to_pose """ header = Header() header.frame_id = self.ik_solver.base_link robot_state = RobotState() robot_state.joint_state.name = self.ik_solver.joint_names robot_state.joint_state.position = joints links = [self.ik_solver.tip_link] return self.fk_solver(header, links, robot_state).pose_stamped[0] def collision_free(self, joints): """ Checks if the configuration collides with anything in the planning scene ------ joints: a list of size 6. Configuration for the collision check ------ returns: boolean True if it's ok, False if it collides """ raiseNotDefined() def add_stl_to_scene(self, id, fileName, pose, scale): """ Adds an STL object to the scene ------ id: object id (string) fileName: location of the STL file pose: autolab rigid_transform? scale: len 3 list defining scale in x,y,z """ raiseNotDefined() def add_box_to_scene(self, id, pose, size): """ Adds a box to the scene ------ id: object id (string) pose: see above size: len 3 list for xyz dimensions """ raiseNotDefined() def populate_scene(): """ Not sure how this will be implemented yet, but will basically use the PathPlanner planning scene functionality to add the scene items needed for our test/demo """ def remove_object_from_scene(self, id): """ removes object from the scene ------ id: object id """ raiseNotDefined() def attach_object_to_robot(self, id): """ attaches object to the robot ------ id: object id """ raiseNotDefined() def detach_object_from_robot(self, id): """ detaches object from robot ------ id: object id """ raiseNotDefined() def visualize_plan(self, plan): # Untested """ Visualize the plan in RViz """ display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = plan.points[0] display_trajectory.trajectory.extend(plan.points) self.display_planned_path_publisher.publish(display_trajectory) def visualize_state(self, joint_state): """ Visualize a joint state in RViz """ raiseNotDefined() def iterate_ik(self, pose, state_list=None, xyz_bounds=None, rpy_bounds=None, iteration_limit=10): """ Iterates IK until a non-colliding solution is found, or it hits the iteration limit ------ pose: see get_ik state_list: list of length 6 lists, each of which is an initial condition to be tested. After the list is exhausted, the system will use pseudorandom start conditions until the iteration count runs out (hopefully it'll pick random states intelligently) xyz_bounds: see get_ik rpy_bounds: see get_ik iteration_limit: max number of initial conditions the algorithm will try (including the list) Default is 10. ------ returns: joint states, a list of size 6 (or None for failure) """ raiseNotDefined() def grasp_plan(self, pregrasp, grasp): """ Plans to a pregrasp pose, then plans to the grasp pose Does not open/close fingers This should be the main function used, and will use many of the other functions It will (hopefully) select intelligent IK solutions ------ pregrasp: An autolab rigid_transform? see plan_to_pose grasp: same as above ------ returns: either two plans concatenated or just executes it """ raiseNotDefined()
class CartesianPoseMoveitPlanner: def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_goal_pose_planner_node') self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander('kuka_arm') #Reference link: https://answers.ros.org/question/255792/change-planner-in-moveit-python/ #self.group.set_planner_id('RRTstarkConfigDefault') self.group.set_planner_id('RRTConnectkConfigDefault') self.group.set_planning_time(10) self.group.set_num_planning_attempts(3) #self.group.set_planning_time(15) #self.group.set_num_planning_attempts(1) self.zero_joint_states = np.zeros(7) self.left_home_pose = False if self.left_home_pose: self.home_joint_states = np.array([ 0.0001086467455024831, 0.17398914694786072, -0.00015721925592515618, -1.0467143058776855, 0.0006054198020137846, -0.00030679398332722485, 3.3859387258416973e-06 ]) else: self.home_joint_states = np.array([ 0.0001086467455024831, -0.17398914694786072, 0.00015721925592515618, 1.0467143058776855, 0.0006054198020137846, -0.00030679398332722485, 3.3859387258416973e-06 ]) self.ik_solver = IK('world', 'allegro_mount') self.seed_state = [0.0] * self.ik_solver.number_of_joints def go_home(self): print 'go home' self.group.clear_pose_targets() self.group.set_joint_value_target(self.home_joint_states) plan_home = self.group.plan() return plan_home def go_zero(self): print 'go zero' self.group.clear_pose_targets() self.group.set_joint_value_target(self.zero_joint_states) plan_home = self.group.plan() return plan_home def go_goal(self, pose): print 'go goal' self.group.clear_pose_targets() self.group.set_pose_target(pose) plan_goal = self.group.plan() return plan_goal def go_goal_trac_ik(self, pose): print 'go goal' self.group.clear_pose_targets() ik_js = self.ik_solver.get_ik(self.seed_state, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) if ik_js is None: rospy.logerr('No IK solution for motion planning!') return None self.group.set_joint_value_target(np.array(ik_js)) plan_goal = self.group.plan() return plan_goal def handle_pose_goal_planner(self, req): plan = None if req.go_home: plan = self.go_home() elif req.go_zero: plan = self.go_zero() else: # plan = self.go_goal(req.palm_goal_pose_world) plan = self.go_goal_trac_ik(req.palm_goal_pose_world) #print plan response = PalmGoalPoseWorldResponse() response.success = False if plan is None: return response if len(plan.joint_trajectory.points) > 0: response.success = True response.plan_traj = plan.joint_trajectory return response def create_moveit_planner_server(self): rospy.Service('moveit_cartesian_pose_planner', PalmGoalPoseWorld, self.handle_pose_goal_planner) rospy.loginfo('Service moveit_cartesian_pose_planner:') rospy.loginfo('Reference frame: %s' % self.group.get_planning_frame()) rospy.loginfo('End-effector frame: %s' % self.group.get_end_effector_link()) rospy.loginfo('Robot Groups: %s' % self.robot.get_group_names()) rospy.loginfo('Ready to start to plan for given palm goal poses.') def handle_arm_movement(self, req): self.group.go(wait=True) response = MoveArmResponse() response.success = True return response def create_arm_movement_server(self): rospy.Service('arm_movement', MoveArm, self.handle_arm_movement) rospy.loginfo('Service moveit_cartesian_pose_planner:') rospy.loginfo('Ready to start to execute movement plan on robot arm.')
from trac_ik_python.trac_ik import IK ik_solver = IK(base_link="base_link", tip_link="tool0", timeout=0.005, epsilon=1e-5, solve_type="Speed", urdf_string=None) seed_state = [0.0] * ik_solver.number_of_joints qa = [-0.131, 0.337, 0.263, -1., 0., 0., -0.019] print(ik_solver.get_ik(seed_state, -0.131, 0.337, 0.263, -1., 0., 0., -0.019)) seed_state = [0.3584, -0.80591, 2.14269, -3.55912, -1.82971, 0.43981] print( ik_solver.get_ik(seed_state, -0.131, 0.269, 0.006, -0.383, 0., 0., -0.924))
class PR2Teleop(object): def __init__(self): self.ik_right = IK("torso_lift_link", #"r_wrist_roll_link") "r_gripper_tool_frame") self.ik_left = IK("torso_lift_link", "l_wrist_roll_link") self.left_command = rospy.Publisher('/l_arm_controller/command', JointTrajectory, queue_size=1) self.right_command = rospy.Publisher('/r_arm_controller/command', JointTrajectory, queue_size=1) self.last_left_pose = None self.left_pose = rospy.Subscriber('/left_controller_as_posestamped', PoseStamped, self.left_cb, queue_size=1) self.last_right_pose = None self.right_pose = rospy.Subscriber('/right_controller_as_posestamped', PoseStamped, self.right_cb, queue_size=1) rospy.sleep(2.0) def left_cb(self, msg): self.last_left_pose = msg def right_cb(self, msg): self.last_right_pose = msg def send_right_arm_goal(self, positions): jt = JointTrajectory() jt.header.stamp = rospy.Time.now() jt.joint_names = ["r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint", "r_wrist_flex_joint", "r_wrist_roll_joint"] jtp = JointTrajectoryPoint() jtp.positions = list(positions) jtp.velocities = [0.0] * len(positions) jtp.time_from_start = rospy.Time(0.4) jt.points.append(jtp) # print("Goal: ") # print(jt) self.right_command.publish(jt) def send_left_arm_goal(self, positions): jt = JointTrajectory() jt.header.stamp = rospy.Time.now() jt.joint_names = ["l_shoulder_pan_joint", "l_shoulder_lift_joint", "l_upper_arm_roll_joint", "l_elbow_flex_joint", "l_forearm_roll_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"] jtp = JointTrajectoryPoint() jtp.positions = list(positions) jtp.velocities = [0.0] * len(positions) jtp.time_from_start = rospy.Time(0.1) jt.points.append(jtp) self.left_command.publish(jt) def run_with_ik(self): qinit = [0., 0., 0., 0., 0., 0., 0.] x = y = z = 0.0 rx = ry = rz = 0.0 rw = 1.0 bx = by = bz = 0.02 brx = bry = brz = 0.5 r = rospy.Rate(4) while not rospy.is_shutdown(): ps = self.last_right_pose if ps is None: r.sleep() print("No last right pose...") continue x = self.last_right_pose.pose.position.x y = self.last_right_pose.pose.position.y z = self.last_right_pose.pose.position.z rx = self.last_right_pose.pose.orientation.x ry = self.last_right_pose.pose.orientation.y rz = self.last_right_pose.pose.orientation.z rw = self.last_right_pose.pose.orientation.w # rospy.loginfo("Got pose: " + str(ps)) sol = None retries = 0 while not sol and retries < 10: sol = self.ik_right.get_ik(qinit, x, y, z, rx, ry, rz, rw, bx, by, bz, brx, bry, brz) retries += 1 if sol: print "Solution found: (" + str(retries) + " retries)" print sol self.send_right_arm_goal(sol) qinit = sol else: print "NO SOLUTION FOUND :(" r.sleep()
class Controller: def __init__(self): # initialize publishers, subscribers, tflisteners self.arm_pub = rospy.Publisher('/goal_dynamixel_position', JointState, queue_size=1) self.pan_pub = rospy.Publisher('/pan/command', Float64, queue_size=1) self.tilt_pub = rospy.Publisher('/tilt/command', Float64, queue_size=1) self.gripper_open_pub = rospy.Publisher('/gripper/open', Empty, queue_size=1) self.gripper_close_pub = rospy.Publisher('/gripper/close', Empty, queue_size=1) rospy.Subscriber('/joint_states', JointState, self.get_joint_state) self.tf_listener = tf.TransformListener() self.history = {'timestamp': deque(), 'joint_feedback': deque()} # global variables self.current_joint_state = None self.current_gripper_state = None self.current_target_state = None # global frames self.GRIPPER_LINK = "gripper_link" self.BOTTOM_PLATE_LINK = "bottom_plate" # other classes self.ik_solver = IK(self.BOTTOM_PLATE_LINK, self.GRIPPER_LINK) # predefined variables self.HOME_POS_MANIPULATOR_00 = [0.0, 0.0, 0.0, 0.0, 0.0] self.HOME_POS_MANIPULATOR_01 = [ 0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784 ] self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -1.57, 1.57] self.HOME_POS_CAMERA_01 = [0.0, 0.698] self.HOME_POS_CAMERA_02 = [-0.452, -0.452] self.IK_POSITION_TOLERANCE = 0.01 self.IK_ORIENTATION_TOLERANCE = np.pi / 3 self.MIN_CLOSING_GAP = 0.002 self.MOVEABLE_JOINTS = [0, 1, 4] self.CONVERGENCE_CRITERION = 0.1 self.CONVERGENCE_CRITERION_COUNT = 10 def set_camera_angles(self, angles): pan_msg = Float64() pan_msg.data = float(angles[0]) rospy.loginfo('Going to camera pan: {} rad'.format(angles[0])) self.pan_pub.publish(pan_msg) tilt_msg = Float64() tilt_msg.data = float(angles[1]) rospy.loginfo('Going to camera tilt: {} rad'.format(angles[1])) self.tilt_pub.publish(tilt_msg) def set_arm_joint_angles(self, joint_target): joint_state = JointState() joint_state.position = tuple(joint_target) self.arm_pub.publish(joint_state) convergence_count = 0 while (convergence_count < self.CONVERGENCE_CRITERION_COUNT): if (np.sum( np.abs( np.asarray(self.current_joint_state) - np.asarray(joint_target))) < self.CONVERGENCE_CRITERION): convergence_count = convergence_count + 1 else: convergence_count = 0 rospy.loginfo("CONVERGED!") def get_joint_state(self, data): # TODO: Change this when required # Add timestamp self.history['timestamp'].append(time.time()) if (len(self.history['timestamp']) > 2): self.history['timestamp'].popleft() # Add Joint Feedback joint_angles = np.array(data.position)[self.MOVEABLE_JOINTS] self.history['joint_feedback'].append(joint_angles) if (len(self.history['joint_feedback']) > 2): self.history['joint_feedback'].popleft() self.current_joint_state = data.position[0:5] self.current_gripper_state = data.position[7:9] def open_gripper(self): empty_msg = Empty() rospy.loginfo('Opening gripper') self.gripper_open_pub.publish(empty_msg) rospy.sleep(4) def close_gripper(self): empty_msg = Empty() rospy.loginfo('Closing gripper') self.gripper_close_pub.publish(empty_msg) rospy.sleep(4) # finds the inverse kinematics solution for the target pose def compute_ik(self, target_pose): """ Parameters ---------- ik_solver: trac_ik_python.trac_ik Ik object target_pose: type geometry_msgs/Pose current_joint: list with length the number of joints (i.e. 5) Returns ---------- IK solution (a list of joint angles for target_pose) if found, None otherwise """ result = self.ik_solver.get_ik( self.current_joint_state, target_pose.position.x, target_pose.position.y, target_pose.position.z, target_pose.orientation.x, target_pose.orientation.y, target_pose.orientation.z, target_pose.orientation.w, self.IK_POSITION_TOLERANCE, self.IK_POSITION_TOLERANCE, self.IK_POSITION_TOLERANCE, self.IK_ORIENTATION_TOLERANCE, self.IK_ORIENTATION_TOLERANCE, self.IK_ORIENTATION_TOLERANCE) if result: rospy.loginfo('IK solution found') else: rospy.logerr('No IK solution found') return result # Goes to the position given by FRAME and grabs the object from the top def go_to_grasp_pose(self, FRAME): try: self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0)) P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logwarn(e) return False O = tf.transformations.euler_from_quaternion(Q) Q = tf.transformations.quaternion_from_euler(0, np.pi / 2, O[2]) poses = [ Pose(Point(P[0], P[1], P[2] + 0.15), Quaternion(Q[0], Q[1], Q[2], Q[3])), Pose(Point(P[0], P[1], P[2] + 0.10), Quaternion(Q[0], Q[1], Q[2], Q[3])) ] print(poses) target_joint = None self.open_gripper() for pose in poses: if self.current_joint_state: target_joint = self.compute_ik(pose) else: print("Joint State Subscriber not working") return False if target_joint: self.set_arm_joint_angles(target_joint) self.current_target_state = np.array(target_joint)[ self.MOVEABLE_JOINTS] else: return False return True # checks if the object was grasped or not def check_grasp(self): if self.current_gripper_state: if(np.abs(self.current_gripper_state[0]) < self.MIN_CLOSING_GAP \ and np.abs(self.current_gripper_state[1] < self.MIN_CLOSING_GAP)): print("Coundn't grasp the object") return False else: return True else: print("Joint State Subscriber not working") return False # Goes to the position given by FRAME and grabs the object from the top def go_to_handover_pose(self, pose): print(pose) try: self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, pose.header.frame_id, rospy.Time.now(), rospy.Duration(5.0)) P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, pose.header.frame_id, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logwarn(e) return False O = tf.transformations.euler_from_quaternion(Q) Q = tf.transformations.quaternion_from_euler(0, -np.pi / 3, O[2]) poses = Pose(Point(P[0], P[1], P[2] - 0.45), Quaternion(Q[0], Q[1], Q[2], Q[3])) target_joint = None while target_joint is None: if self.current_joint_state: target_joint = self.compute_ik(poses) else: print("Joint State Subscriber not working") return False if target_joint: print(target_joint) self.set_arm_joint_angles(target_joint) self.current_target_state = np.array(target_joint)[ self.MOVEABLE_JOINTS] else: rospy.logwarn("No Solution was found. Current Tolerance " + str(self.IK_POSITION_TOLERANCE)) self.IK_POSITION_TOLERANCE += 0.05 print(target_joint) return True # Goes to the position given by FRAME and grabs the object from the top def go_to_data_collection_pose(self, pose): # print(pose) # poses = Pose(Point(pose[0], pose[1], pose[2]), Quaternion(pose[3], pose[4], pose[5], pose[6])) target_joint = None while target_joint is None: if self.current_joint_state: target_joint = self.compute_ik(pose) else: print("Joint State Subscriber not working") return False if target_joint: print(target_joint) self.set_arm_joint_angles(target_joint) self.current_target_state = np.array(target_joint)[ self.MOVEABLE_JOINTS] else: rospy.logwarn("No Solution was found. Current Tolerance " + str(self.IK_POSITION_TOLERANCE)) self.IK_POSITION_TOLERANCE += 0.05 print(target_joint) return True
class PR2Teleop(object): def __init__(self): urdf = rospy.get_param('/robot_description') self.ik_right = IK("base_link", "ee_link", 0.005, 1e-5, "Distance", urdf) #self.ik_left = IK("torso_lift_link", # "l_wrist_roll_link") #self.left_command = rospy.Publisher('/l_arm_controller/command', # JointTrajectory, # queue_size=1) self.right_command = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=1) #self.last_left_pose = None #self.left_pose = rospy.Subscriber('/left_controller_as_posestamped', #PoseStamped, #self.left_cb, queue_size=1) self.last_right_pose = None self.right_pose = rospy.Subscriber( '/free_positioning/gripper_marker_pose', PoseStamped, self.right_cb, queue_size=1) rospy.sleep(2.0) def left_cb(self, msg): self.last_left_pose = msg def right_cb(self, msg): self.last_right_pose = msg def send_right_arm_goal(self, positions): jt = JointTrajectory() jt.header.stamp = rospy.Time.now() jt.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] jtp = JointTrajectoryPoint() jtp.positions = list(positions) jtp.velocities = [0.0] * len(positions) jtp.time_from_start = rospy.Time(0.08) #default 0.4 jt.points.append(jtp) # print("Goal: ") #print(jt) self.right_command.publish(jt) ''' def send_left_arm_goal(self, positions): jt = JointTrajectory() jt.header.stamp = rospy.Time.now() jt.joint_names = ["l_shoulder_pan_joint", "l_shoulder_lift_joint", "l_upper_arm_roll_joint", "l_elbow_flex_joint", "l_forearm_roll_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"] jtp = JointTrajectoryPoint() jtp.positions = list(positions) jtp.velocities = [0.0] * len(positions) jtp.time_from_start = rospy.Time(0.1) jt.points.append(jtp) self.left_command.publish(jt) ''' def run_with_ik(self): qinit = [0., 0., 0., 0., 0., 0.] x = y = z = 0.0 rx = ry = rz = 0.0 rw = 1.0 bx = by = bz = 0.0 #default 0.02 brx = bry = brz = 0.0 #default 0.5 r = rospy.Rate(125) #default 4 while not rospy.is_shutdown(): ps = self.last_right_pose if ps is None: r.sleep() print("No last right pose...") continue x = self.last_right_pose.pose.position.x y = self.last_right_pose.pose.position.y z = self.last_right_pose.pose.position.z rx = self.last_right_pose.pose.orientation.x ry = self.last_right_pose.pose.orientation.y rz = self.last_right_pose.pose.orientation.z rw = self.last_right_pose.pose.orientation.w # rospy.loginfo("Got pose: " + str(ps)) sol = None retries = 0 start = time.clock() while not sol and retries < 10: sol = self.ik_right.get_ik(qinit, x, y, z, rx, ry, rz, rw, bx, by, bz, brx, bry, brz) retries += 1 end = time.clock() print "Execution time:" + str(1000 * (end - start)) + "ms" if sol: print "Solution found: (" + str(retries) + " retries)" #print sol self.send_right_arm_goal(sol) qinit = sol else: print "NO SOLUTION FOUND :(" r.sleep()
ind += 1 #first seed is zero-vector for trajectory evaluation if use_last_solution_as_seed: seed_state = np.zeros_like(seed_configurations[0,:]) #solve for validation data joints_num_val = np.zeros((X_val.shape[0], ik_solver.number_of_joints)) sample_times = [] for i in range(joints_num_val.shape[0]): if not use_last_solution_as_seed: seed_state = seed_configurations[i,:] begin = timer() joints_i = ik_solver.get_ik(seed_state, #initial configuration X_val[i,0], X_val[i,1], 0.0, #desired position (x, y, z) 0.0, 0.0, math.sin(X_val[i,2]/2), math.cos(X_val[i,2]/2), #desired orientation (quaternions: xq, yq, zq, wq) pos_error, pos_error, pos_error, #allowed position error (x, y, z) ori_error, ori_error, ori_error) #allowed orientation error (rotation over axis x, y, z) end = timer() sample_times.append(end - begin) joints_num_val[i,:] = joints_i if use_last_solution_as_seed and not np.any(np.isnan(joints_num_val[i,:])): seed_state = joints_num_val[i,:] #for trajectory evaluation: seed is always the current configuration (last solution) print("Sum TRAC-IK: "+str(np.sum(sample_times))+" seconds") print("Mean TRAC-IK: "+str(np.mean(sample_times))+" seconds") print("Std TRAC-IK: "+str(np.std(sample_times))+" seconds") np.save("data/joints_num_val.npy", joints_num_val)
class CtrlEndEffPos(object): """Control end effector position Attributes hebi_group_name (str): hebi_families (list of str): HEBI actuator families [base,..., tip] hebi_names (list of str): HEBI actuator names [base,..., tip] from_master_topic (str): to_master_topic (str): base_link_name (str): arm base link name in urdf end_link_name (str): end base link name in urdf ik_timeout (float): ik_epsilon (float): ik_solve_type (str): https://bitbucket.org/traclabs/trac_ik/src/HEAD/trac_ik_lib/ ik_position_bounds (list of float): [x_pos_bound, y_pos_bound, z_pos_bound] """ def __init__(self, hebi_group_name, hebi_families, hebi_names, from_master_topic, to_master_topic, base_link_name, end_link_name, ik_timeout, ik_epsilon, ik_solve_type, ik_position_bounds): self.hebi_mapping = [] self.current_jt_pos = {} self.current_jt_vel = {} self.current_jt_eff = {} self._last_valid_jt_ang = [] self._msg_from_master = None ## ROS node setup rospy.init_node('end_eff_pos_ctrl_node') # Should be renamed in .launch self._rate = rospy.Rate(200) #NOTE: Could set via arg ## HEBI setup - NOTE: hebiros_node must be running self.hebi_group_name = hebi_group_name rospy.loginfo("HEBI Group name: "+ str(hebi_group_name)) self.hebi_families = hebi_families rospy.loginfo("HEBI families: "+ str(hebi_families)) self.hebi_names = hebi_names rospy.loginfo("HEBI names: "+ str(hebi_names)) self.hebi_mapping = [family+'/'+name for family,name in zip(hebi_families,hebi_names)] rospy.loginfo("self.hebi_mapping: "+ str(self.hebi_mapping)) # Create a service client to create HEBI group set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names', AddGroupFromNamesSrv) # Topic to receive feedback from HEBI group hebi_group_feedback_topic = "/hebiros/"+hebi_group_name+"/feedback/joint_state" # Topic to send commands to HEBI group hebi_group_command_topic = "/hebiros/"+hebi_group_name+"/command/joint_state" # Call the /hebiros/add_group_from_names service to create a group rospy.wait_for_service('/hebiros/add_group_from_names') set_hebi_group(hebi_group_name,hebi_names,hebi_families) ## ROS pub/sub setup self._master_sub = rospy.Subscriber(from_master_topic, Pose, self._master_cb) self._master_pub = rospy.Publisher(to_master_topic, String, queue_size=1) self._arm_sub = rospy.Subscriber(hebi_group_feedback_topic, JointState, self._arm_cb) self._arm_pub = rospy.Publisher(hebi_group_command_topic, JointState, queue_size=1) ## Simulation self._base_pub = rospy.Publisher("/to_x5_base", Float32, queue_size=1) self._shoulder_pub = rospy.Publisher("/to_x5_shoulder", Float32, queue_size=1) self._elbow_pub = rospy.Publisher("/to_x5_elbow", Float32, queue_size=1) ## trac_ik setup # Get urdf from ROS parameter server urdf_str = "" urdf_loaded = False while not rospy.is_shutdown() and not urdf_loaded: if rospy.has_param('/robot_description'): #NOTE: Could set via arg urdf_str = rospy.get_param('/robot_description') urdf_loaded = True rospy.loginfo("Pulled /robot_description from parameter server.") else: rospy.sleep(0.2) # sleep for 0.2s of ROS time self._ik_solver = IK(base_link_name, end_link_name, urdf_string=urdf_str, timeout=ik_timeout, epsilon=ik_epsilon, solve_type=ik_solve_type) rospy.loginfo(" self.ik_solver.base_link: %s", self._ik_solver.base_link) rospy.loginfo(" self.ik_solver.tip_link: %s", self._ik_solver.tip_link) rospy.loginfo(" self.ik_solver.link_names: %s", self._ik_solver.link_names) rospy.loginfo(" self.ik_solver.joint_names: %s", self._ik_solver.joint_names) lb, ub = self._ik_solver.get_joint_limits() rospy.loginfo(" self.ik_solver.get_joint_limits()") rospy.loginfo(" lower_bounds: %s", lb) rospy.loginfo(" upper_bounds: %s", ub) self._xyz_position_bounds = ik_position_bounds rospy.loginfo(" self._xyz_position_bounds: %s", self._xyz_position_bounds) self._orientation_bounds = [31416.0, 31416.0, 31416.0] #NOTE: This implements position-only IK rospy.loginfo(" self._orientation_bounds: %s", self._orientation_bounds) ## pykdl_utils setup robot_urdf = URDF.from_xml_string(urdf_str) self._kdl_kin = KDLKinematics(robot_urdf, base_link_name, end_link_name) # Wait for connections to be setup while not rospy.is_shutdown() and len(self.current_jt_pos) < len(self.hebi_mapping): rospy.sleep(0.2) # sleep for 0.2s of ROS time jt_angles = [self.current_jt_pos[motor] for motor in self.hebi_mapping] pose = self._kdl_kin.forward(jt_angles) rospy.loginfo("current pose: %s", pose) self.current_eff_pos = [round(pose[0,3], 5), round(pose[1,3], 5), round(pose[2,3], 5)] rospy.loginfo("self.current_eff_pos [x, y, z]: %s", self.current_eff_pos) #TODO: Probably should include pose information too!!! # Would need to convert homogeneous matrix to proper normalized quaternion. def start(self): ## main loop max_step = 0.01 # m - NOTE: Could set via message field while not rospy.is_shutdown(): if self._msg_from_master is None: if self._last_valid_jt_ang is not None: msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = self.hebi_mapping msg.position = self._last_valid_jt_ang msg.velocity = [] #TODO msg.effort = [] #TODO: Gravity compensation self._arm_pub.publish(msg) else: target_pos = [self._msg_from_master.position.x, self._msg_from_master.position.y, self._msg_from_master.position.z] rospy.loginfo(" target_pos: %s", target_pos) self._msg_from_master = None squared_dif = (target_pos[0]-self.current_eff_pos[0])**2.0 \ + (target_pos[1]-self.current_eff_pos[1])**2.0 \ + (target_pos[2]-self.current_eff_pos[2])**2.0 if squared_dif < 1e-6: rospy.loginfo(" Target end-effector position is same as current position!\n" + " Please try another pt...") else: straight_line_dist = m.sqrt(squared_dif) rospy.loginfo(" straight_line_dist: %s [m]", straight_line_dist) num_pts = int(m.ceil(straight_line_dist/max_step)) rospy.loginfo(" num_pts: %s", num_pts) x_dif = target_pos[0]-self.current_eff_pos[0] y_dif = target_pos[1]-self.current_eff_pos[1] z_dif = target_pos[2]-self.current_eff_pos[2] x_delta = x_dif / num_pts y_delta = y_dif / num_pts z_delta = z_dif / num_pts xyz_pts=[] for i in range(num_pts): pt = (self.current_eff_pos[0] + (i+1)*x_delta, self.current_eff_pos[1] + (i+1)*y_delta, self.current_eff_pos[2] + (i+1)*z_delta) xyz_pts.append(pt) wxyz_pts = [(0,0,0,1) for pt in xyz_pts] #NOTE: Temporary - still only interested in position-only IK result, joint_trajectory = self._generate_joint_trajectory(xyz_pts=xyz_pts, wxyz_pts=wxyz_pts) if not result: rospy.loginfo(" IK FAILED! Joint solution not found.\n" + " Please try another pt...") self._master_pub.publish("failure") else: rospy.loginfo(" IK SUCCESS! Joint solution found.") rospy.loginfo(" Executing joint_trajectory...") avg_vel = 0.25 #NOTE: This is not a good way to do this. I need to use some kind of trajectory filter. time_to_execute = straight_line_dist / avg_vel self._execute_joint_trajectory(joint_trajectory, time_to_execute) rospy.loginfo(" Finished executing joint_trajectory.") self._master_pub.publish("success") pose = self._kdl_kin.forward(self._last_valid_jt_ang) self.current_eff_pos = [round(pose[0,3], 5), round(pose[1,3], 5), round(pose[2,3], 5)] # Would need to convert homogeneous matrix to proper normalized quaternion. rospy.loginfo("self.current_eff_pos [x, y, z]: %s", self.current_eff_pos) self._rate.sleep() def _arm_cb(self, msg): for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity, msg.effort): if name not in self.hebi_mapping: print("WARNING: arm_callback - unrecognized name!!!") else: self.current_jt_pos[name] = pos self.current_jt_vel[name] = vel self.current_jt_eff[name] = eff def _master_cb(self, msg): self._msg_from_master = msg def _generate_joint_trajectory(self, xyz_pts, wxyz_pts): seed_state = [self.current_jt_pos[motor] for motor in self.hebi_mapping] joint_trajectory = [] for pos,rot in zip(xyz_pts,wxyz_pts): rospy.loginfo(" seed_state: %s", seed_state) target_jt_ang = self._ik_solver.get_ik(seed_state, pos[0],pos[1],pos[2], rot[0],rot[1],rot[2],rot[3], self._xyz_position_bounds[0], self._xyz_position_bounds[1], self._xyz_position_bounds[2], self._orientation_bounds[0], self._orientation_bounds[1], self._orientation_bounds[2]) rospy.loginfo(" target_jt_ang: %s", target_jt_ang) if target_jt_ang is None: return False, None joint_trajectory.append(target_jt_ang) seed_state = target_jt_ang # for next iteration # Maintain current position while planning - TODO: better to replace with HEBI Trajectory Action? if self._last_valid_jt_ang is not None: msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = self.hebi_mapping msg.position = self._last_valid_jt_ang msg.velocity = [] msg.effort = [] self._arm_pub.publish(msg) return True, joint_trajectory def _execute_joint_trajectory(self, joint_trajectory, time_to_execute): num_pts = len(joint_trajectory) time_start = rospy.Time.now().to_sec() # works with simulation time too time_end = time_start + time_to_execute time_incr = (time_end - time_start) / float(num_pts) index = 0 done = False while not rospy.is_shutdown() and not done: time_now = rospy.Time.now().to_sec() if time_now > time_end: done = True else: if time_now > time_start + index*time_incr: if index < num_pts-1: index += 1 msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = self.hebi_mapping msg.position = joint_trajectory[index] msg.velocity = [] #TODO msg.effort = [] #TODO: Gravity compensation self._arm_pub.publish(msg) ## Simulation self._base_pub.publish(joint_trajectory[index][0]) self._shoulder_pub.publish(joint_trajectory[index][1]) self._elbow_pub.publish(joint_trajectory[index][2]) self._rate.sleep() self._last_valid_jt_ang = joint_trajectory[index]
def main(): rospy.init_node('Joint_Control', anonymous='True') robot_controller = RobotController() rate = rospy.Rate(60) with open('/home/kurt/Robotics/src/ar3/urdf/AR3_noGazebo.urdf', 'r') as fp: urdf = fp.read() Solver = IK("world", "flange", urdf_string=urdf) print("IK solver uses link chain:") print(Solver.link_names) print("IK solver base frame:") print(Solver.base_link) print("IK solver tip link:") print(Solver.tip_link) print("IK solver for joints:") print(Solver.joint_names) # print("IK solver using joint limits:") # lb, up = Solver.get_joint_limits() # print("Lower bound: " + str(lb)) # print("Upper bound: " + str(up)) qinit = [0.0] * Solver.number_of_joints # print(Solver.number_of_joints) x = 0.0 y = -0.3 z = 0.35 # quat = quaternion_from_euler(0.0,3.14,0.0,'rxyz') rx, ry, rz, rw = 1, 0, 0, .007 bx = by = bz = .03 brx = bry = brz = 2 * pi * .1 # sol = Solver.get_ik(qinit,x,y,z,rx,ry,rz,rw,bx,by,bz,brx,bry,brz) sol = Solver.get_ik(qinit, x, y, z, rx, ry, rz, rw) # sol = _ik_solver.CartToJnt(qinit,x,y,z,rx,ry,rz,rw) if not sol: print('No solution found!') return print("Solution found: ", sol) # userIn = raw_input("Can the robot begin moving? Ensure the space is clear (y/n): ") # assert userIn=='y','Robot wont begin moving.' robot_controller.AR3Control.home = 0 robot_controller.AR3Control.run = 1 robot_controller.AR3Control.rest = 0 while not rospy.is_shutdown(): angs = list(sol) if angs: # angs.pop() robot_controller.AR3Control.joint_angles = angs robot_controller.send_joints()
class PathPlanner(object): def __init__(self): """ Path Planner Class. References: dynamicreplanning.weebly.com, moveit python interface tutorial, trac_ik python tutorial jaco_manipulation """ rospy.loginfo("To stop project CTRL + C") rospy.on_shutdown(self.shutdown) # Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Instatiate the move group self.group = moveit_commander.MoveGroupCommander('arm') self.group.set_planning_time(5) self.group.set_workspace([-3, -3, -3, 3, 3, 3]) # This publishes trajectories to RVIZ for visualization self.display_planned_path_publisher = rospy.Publisher( 'arm/display_planned_path', DisplayTrajectory, queue_size=10) # This publishes updates to the planning scene self.planning_scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) # Planning scene self.scene = moveit_commander.PlanningSceneInterface() # RobotCommander self.robot = moveit_commander.RobotCommander() # IK Solver with trac_ik # NOTE: Get this from the MoveGroup so it's adaptable to other robots self.ik_solver = IK('root', 'm1n6s300_end_effector') self.ik_default_seed = [0.0] * self.ik_solver.number_of_joints # FK Solver rospy.wait_for_service('/compute_fk') self.fk_solver = rospy.ServiceProxy('/compute_fk', GetPositionFK) rospy.sleep(2) rate = rospy.Rate(10) def shutdown(self): rospy.loginfo("Stopping project") rospy.sleep(1) def plan_to_config(self, end_state): """ Uses MoveIt to plan a path from the current state to end_state and returns it end_state: list of 6 joint values """ joint_state = JointState() joint_state.header.stamp = rospy.Time.now() # the robot has a dumb base_link joint that we don't want joint_state.name = self.group.get_joints()[1:-1] print joint_state.name joint_state.position = end_state self.group.set_start_state_to_current_state() try: self.group.set_joint_value_target(joint_state) except moveit_commander.MoveItCommanderException: pass # Plan the path plan = self.group.plan() return plan def execute_path(self, path, wait_bool=True): """ Executes a provided path. Note that the current position must be the same as the path's initial position. This is currently not checked. """ self.group.execute(path, wait=wait_bool) # def collision_free_move_pose(self, end_pose): # """ # Uses MoveIt to plan a path from the current state to end effector pose end_pose # end_pose: a PoseStamped object for the end effector # """ # self.group.set_start_state_to_current_state() # self.group.set_joint_value_target(end_pose) # self.group.set_workspace([-3, -3, -3, 3, 3, 3]) # plan = self.group.plan() # return plan def move_home(self): """ Uses MoveIt to plan a path from the current state to the home position """ self.group.set_start_state_to_current_state() self.group.set_named_target('Home') self.group.set_workspace([-3, -3, -3, 3, 3, 3]) plan = self.group.plan() return plan def visualize_plan(self, plan): """ Visualize the plan in RViz """ display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = plan.points[0] display_trajectory.trajectory.extend(plan.points) self.display_planned_path_publisher.publish(display_trajectory) def make_pose(self, position, orientation, frame): """ Creates a PoseStamped message based on provided position and orientation position: list of size 3 orientation: list of size 4 (quaternion) (wxyz) frame: string (the reference frame to which the pose is defined) returns pose: a PoseStamped object """ pose = PoseStamped() pose.header.frame_id = frame pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] pose.pose.orientation.w = orientation[0] pose.pose.orientation.x = orientation[1] pose.pose.orientation.y = orientation[2] pose.pose.orientation.z = orientation[3] return pose def get_ik(self, pose, seed_state=None, xyz_bounds=None, rpy_bounds=None): """ get_ik returns a joint configuration from an end effector pose by using the trac_ik solver pose: PoseStamped object. The header.frame_id should be "root", or it won't work seed_state: a list of size 6. Initial joint positions for the solver. Default is [0,0,0,0,0,0] xyz_bounds: a list of size 3. xyz bounds of the end effector in meters. This allows an approximate ik solution. Default is None rpy_bounds: a list of size 3. roll pitch yaw bounds of the end effector in radians. This allows an approximate ik solution. Default is None returns state: a list of joint values """ if seed_state is None: seed_state = self.ik_default_seed if pose.header.frame_id != self.ik_solver.base_link: raise ValueError("Frame ID is not the root link") position = pose.pose.position orientation = pose.pose.orientation print seed_state, position.x, position.y, \ position.z, orientation.x, orientation.y, \ orientation.z, orientation.w \ if xyz_bounds is None or rpy_bounds is None: state = self.ik_solver.get_ik(seed_state, position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w) else: state = self.ik_solver.get_ik( seed_state, position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w, xyz_bounds[0], xyz_bounds[1], xyz_bounds[2], rpy_bounds[0], rpy_bounds[1], rpy_bounds[2]) return state def get_fk(self, joints): """ Gets forward kinematics to the end effector joints: size 6 list. Joint angles for desired pose returns pose: StackedPose of the end effector in the 'root' frame """ header = Header() header.frame_id = self.ik_solver.base_link robot_state = RobotState() robot_state.joint_state.name = self.ik_solver.joint_names robot_state.joint_state.position = joints links = [self.ik_solver.tip_link] return self.fk_solver(header, links, robot_state).pose_stamped[0]
NUM_COORDS = 200 rand_coords = [] for _ in range(NUM_COORDS): x = random() * 0.5 y = random() * 0.6 + -0.3 z = random() * 0.7 + -0.35 rand_coords.append((x, y, z)) # Check some random coords with fixed orientation avg_time = 0.0 num_solutions_found = 0 for x, y, z in rand_coords: ini_t = time.time() sol = ik_solver.get_ik(qinit, x, y, z, rx, ry, rz, rw, bx, by, bz, brx, bry, brz) fin_t = time.time() call_time = fin_t - ini_t # print("IK call took: " + str(call_time)) avg_time += call_time if sol: # print("X, Y, Z: " + str( (x, y, z) )) # print("SOL: " + str(sol)) num_solutions_found += 1 avg_time = avg_time / NUM_COORDS print() print("Found " + str(num_solutions_found) + " of 200 random coords") print("Average IK call time: " + str(avg_time))
class GCodeGenerator(): def __init__(self): rospy.init_node('gcode_generator', anonymous='True') self.rospack = rospkg.RosPack() self.ar3_path = self.rospack.get_path('ar3') self.program_buffer = [] self.expected_len = 0 self.previous_pose = [0.0] * 6 with open(self.ar3_path + '/urdf/ar3.urdf', 'r') as fp: urdf = fp.read() self.solver = IK("world", "tcp", urdf_string=urdf) def save_queue(self): print('Saving queue.') fp = open(self.ar3_path + '/src/ar3/programs/test.txt', 'w') for line in self.program_buffer: fp.write(line + '\n') fp.close() def queue_home_config(self): self.expected_len += 1 home_config = [0.0] * 6 self.add_to_queue(home_config) def queue_rest_config(self): self.expected_len += 1 rest_angs = [0.007, 0.585, 1.020, -0.002, 1.294, 0.007] self.add_to_queue(rest_angs) def add_to_queue(self, angles): program_line = '' program_line += 'JMove' for angle in angles: program_line += ' %.3f' % angle gripper_angle = 0 speed = 0.95 program_line += ' %d' % gripper_angle program_line += ' %.2f' % speed self.program_buffer.append(program_line) def add_ik_to_queue(self, coord): self.expected_len += 1 angs = self.get_ik(coord) if angs: self.add_to_queue(angs) def get_ik(self, coord): x, y, z = coord[0], coord[1], coord[2] quat = quaternion_from_euler(coord[3], coord[4], coord[5], 'rxyz') rx, ry, rz, rw = quat self.qinit = self.previous_pose angles = self.solver.get_ik(self.qinit, x, y, z, rx, ry, rz, rw) if not angles: print("Solution not found!") return 0 else: print("Solution found!") self.previous_pose = angles return list(angles)
ordered_joints = [ dictlist[5], dictlist[6], dictlist[3], dictlist[4], dictlist[0], dictlist[1], dictlist[2] ] # print ordered_joints ### SEEDING CURRENT JOINT POSITIONS INTO THE SOLVER seed_state = ordered_joints # seed_state = [0,0,0,0,0,0,0] # print seed_state # joints_from_ik1 = ik_solver.get_ik(seed_state, 0.6366221863461569, 0.8497028752234516, 0.05416354881648494, -0.3823911659266366, 0.9223729088465533, 0.023089273180701524, 0.04972020425543535)#, bx, by, bz, brx, bry, brz) # QX, QY, QZ, QW , , joints_from_ik1 = ik_solver.get_ik(seed_state, xl, yl, zl, Qwl, Qxl, Qyl, Qzl, bx, by, bz, brx, bry, brz) # QX, QY, QZ, QW , , # ik_solver.CartToJnt(qinit, # x, y, z, # rx, ry, rz, rw, # bx, by, bz, # brx, bry, brz) # print "IK solver uses link chain:" # print ik_solver.link_names print joints_from_ik1, 'Joint Solutions' # positions = dict(zip(left.joint_names(), list(joints_from_ik1))) # left.move_to_joint_positions(positions, timeout=10.0, threshold=0.008726646) # break
class boris_kinematics(object): def __init__(self, root_link=None, tip_link="left_arm_7_link"): self._urdf = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._urdf) self._base_link = self._urdf.get_root( ) if root_link is None else root_link self._tip_link = tip_link self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._joint_names = self._urdf.get_chain(self._base_link, self._tip_link, links=False, fixed=False) self._num_jnts = len(self._joint_names) # KDL self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) # trac_ik urdf_str = rospy.get_param('/robot_description') self.trac_ik_solver = IK(self._base_link, self._tip_link, urdf_string=urdf_str) self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_types = [] for jnt_name in self._joint_names: jnt = self._urdf.joint_map[jnt_name] if jnt.limit is not None: self.joint_limits_lower.append(jnt.limit.lower) self.joint_limits_upper.append(jnt.limit.upper) else: self.joint_limits_lower.append(None) self.joint_limits_upper.append(None) self.joint_types.append(jnt.type) self._default_seed = [ -0.3723750412464142, 0.02747996523976326, -0.7221865057945251, -1.69843590259552, 0.11076358705759048, 0.5450965166091919, 0.45358774065971375 ] # home_pos #lower_lim = np.where(np.isfinite(self.joint_limits_lower), self.joint_limits_lower, 0.) #upper_lim = np.where(np.isfinite(self.joint_limits_upper), self.joint_limits_upper, 0.) #seed = (lower_lim + upper_lim) / 2.0 #self._default_seed = np.where(np.isnan(seed), [0.]*len(seed), seed) # mid of the joint limits def forward_kinematics(self, joint_values): kdl_array = joint_list_to_kdl(joint_values) end_frame = PyKDL.Frame() self._fk_p_kdl.JntToCart(kdl_array, end_frame) pos = end_frame.p quat = PyKDL.Rotation(end_frame.M).GetQuaternion() return np.array([pos[0], pos[1], pos[2] ]), np.array([quat[0], quat[1], quat[2], quat[3]]) def trac_ik_inverse_kinematics(self, pos, quat, seed=None): if seed is None: seed = self._default_seed result = self.trac_ik_solver.get_ik(seed, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]) # return np.array(result) return True if result is not None else False def trac_ik_inverse_kinematics2(self, pos, quat, seed=None): if seed is None: seed = self._default_seed result = self.trac_ik_solver.get_ik(seed, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]) # return np.array(result) if result is not None else None def kdl_inverse_kinematics(self, pos, quat, seed=None): pos = PyKDL.Vector(pos[0], pos[1], pos[2]) rot = PyKDL.Rotation() rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3]) goal_pose = PyKDL.Frame(rot, pos) seed_array = joint_list_to_kdl(self._default_seed) if seed != None: seed_array = joint_list_to_kdl(seed) result_angles = PyKDL.JntArray(self._num_jnts) if self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) # return result return True else: # return None return False def kdl_inverse_kinematics_jl(self, pos, quat, seed=None, min_joints=None, max_joints=None): pos = PyKDL.Vector(pos[0], pos[1], pos[2]) rot = PyKDL.Rotation() rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3]) goal_pose = PyKDL.Frame(rot, pos) if min_joints is None: min_joints = self.joint_limits_lower if max_joints is None: max_joints = self.joint_limits_upper mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) ik_jl_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl) seed_array = joint_list_to_kdl(self._default_seed) if seed != None: seed_array = joint_list_to_kdl(seed) result_angles = PyKDL.JntArray(self._num_jnts) if ik_jl_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) # return result return True else: # return None return False def kdl_inverse_kinematics_jl2(self, pos, quat, seed=None, min_joints=None, max_joints=None): pos = PyKDL.Vector(pos[0], pos[1], pos[2]) rot = PyKDL.Rotation() rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3]) goal_pose = PyKDL.Frame(rot, pos) if min_joints is None: min_joints = self.joint_limits_lower if max_joints is None: max_joints = self.joint_limits_upper mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) ik_jl_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl) seed_array = joint_list_to_kdl(self._default_seed) if seed != None: seed_array = joint_list_to_kdl(seed) result_angles = PyKDL.JntArray(self._num_jnts) if ik_jl_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) # return result return result else: # return None return None
class AR3Controller(QMainWindow): def __init__(self, screen): QMainWindow.__init__(self) rospy.init_node('ar3_ui', anonymous='True') self.setWindowTitle("AR3 Controller") width = 1200 height = 900 self.setGeometry(0, 0, width, height) self.rospack = rospkg.RosPack() self.ar3_path = self.rospack.get_path('ar3') uic.loadUi(self.ar3_path + '/src/ar3/ui/ar3_controller.ui', self) self.joint_jog_widget = uic.loadUi(self.ar3_path + '/src/ar3/ui/joint_controller.ui') self.pose_jog_widget = uic.loadUi(self.ar3_path + '/src/ar3/ui/pose_controller.ui') with open(self.ar3_path + '/urdf/ar3.urdf', 'r') as fp: urdf = fp.read() self.solver = IK("world", "tcp", urdf_string=urdf) self.qinit = [0.0] * self.solver.number_of_joints self.connected = False self.ar3_feeback_sub = rospy.Subscriber('/AR3/Feedback', ar3_feedback, self.update_feedback_label) self.listener = tf.TransformListener() self.robot_controller = RobotController() self.robot_controller.AR3Control.run = 1 self.robot_controller.AR3Control.accelerate = 1 self.feedback_angles = [] self.setpoint_angles = [] self.gripper_angle = self.gripper_angle_spinbox.value() # Move Commands self.move_to_pose_button.clicked.connect(self.move_to_pose) self.move_to_rest_button.clicked.connect(self.move_to_rest) self.move_to_home_button.clicked.connect(self.move_to_home) self.move_gripper_button.clicked.connect(self.move_gripper) self.pull_current_config_button.clicked.connect( self.pull_current_config) self.joint_spinboxes = [ self.joint_jog_widget.joint_1_setpoint, self.joint_jog_widget.joint_2_setpoint, self.joint_jog_widget.joint_3_setpoint, self.joint_jog_widget.joint_4_setpoint, self.joint_jog_widget.joint_5_setpoint, self.joint_jog_widget.joint_6_setpoint ] self.pose_spinboxes = [ self.pose_jog_widget.x_spinbox, self.pose_jog_widget.y_spinbox, self.pose_jog_widget.z_spinbox, self.pose_jog_widget.rx_spinbox, self.pose_jog_widget.ry_spinbox, self.pose_jog_widget.rz_spinbox ] # Program Operations self.program_buffer = [] self.cursor_idx = -1 self.add_to_queue_button.clicked.connect(self.add_to_queue) self.start_queue_button.clicked.connect(self.start_queue) self.clear_queue_button.clicked.connect(self.clear_queue) self.program_up_button.clicked.connect(self.program_up) self.program_down_button.clicked.connect(self.program_down) self.program_remove_button.clicked.connect(self.program_remove) self.add_program_wait_button.clicked.connect(self.add_wait_to_queue) # Jog Panel self.timer = QTimer() self.timer.timeout.connect(self.jog_joints) self.jog_rate = 200 self.jog_type = 0 self.joint_jog_idx = 0 self.joint_jog_dir = 1.0 self.joint_radiobutton.toggled.connect(self.set_jog_type) self.pose_radiobutton.toggled.connect(self.set_jog_type) self.jog_layout.addWidget(self.joint_jog_widget) self.jog_layout.addWidget(self.pose_jog_widget) self.pose_jog_widget.hide() self.cartesian_jog_radiobutton.toggled.connect(self.switch_jog_type) self.joint_jog_radiobutton.toggled.connect(self.switch_jog_type) self.ax_labels = [ self.ax1_label, self.ax2_label, self.ax3_label, self.ax4_label, self.ax5_label, self.ax6_label ] self.jog_x_pos_button.pressed.connect(self.start_jog_x_pos) self.jog_x_pos_button.released.connect(self.stop_jogging) self.jog_x_neg_button.pressed.connect(self.start_jog_x_neg) self.jog_x_neg_button.released.connect(self.stop_jogging) self.jog_y_pos_button.pressed.connect(self.start_jog_y_pos) self.jog_y_pos_button.released.connect(self.stop_jogging) self.jog_y_neg_button.pressed.connect(self.start_jog_y_neg) self.jog_y_neg_button.released.connect(self.stop_jogging) self.jog_z_pos_button.pressed.connect(self.start_jog_z_pos) self.jog_z_pos_button.released.connect(self.stop_jogging) self.jog_z_neg_button.pressed.connect(self.start_jog_z_neg) self.jog_z_neg_button.released.connect(self.stop_jogging) self.jog_rx_pos_button.pressed.connect(self.start_jog_rx_pos) self.jog_rx_pos_button.released.connect(self.stop_jogging) self.jog_rx_neg_button.pressed.connect(self.start_jog_rx_neg) self.jog_rx_neg_button.released.connect(self.stop_jogging) self.jog_ry_pos_button.pressed.connect(self.start_jog_ry_pos) self.jog_ry_pos_button.released.connect(self.stop_jogging) self.jog_ry_neg_button.pressed.connect(self.start_jog_ry_neg) self.jog_ry_neg_button.released.connect(self.stop_jogging) self.jog_rz_pos_button.pressed.connect(self.start_jog_rz_pos) self.jog_rz_pos_button.released.connect(self.stop_jogging) self.jog_rz_neg_button.pressed.connect(self.start_jog_rz_neg) self.jog_rz_neg_button.released.connect(self.stop_jogging) self.actionSave_Queue.triggered.connect(self.save_queue) self.actionLoad_Queue.triggered.connect(self.load_queue) # Feedback Labels self.tcp_pose = None self.joint_lcds = [ self.j1_lcd, self.j2_lcd, self.j3_lcd, self.j4_lcd, self.j5_lcd, self.j6_lcd ] self.tcp_lcds = [ self.x_lcd, self.y_lcd, self.z_lcd, self.rx_lcd, self.ry_lcd, self.rz_lcd ] self.show() self.set_jog_type() self.switch_jog_type() if not self.connected: self.frame_3.setEnabled(False) ''' Menu Bar Actions ''' def save_queue(self): print('Saving queue...') fp = open(self.ar3_path + '/src/ar3/programs/test.txt', 'w') for line in self.program_buffer: fp.write(line + '\n') fp.close() def load_queue(self): print('Loading queue...') self.clear_queue() fp = open(self.ar3_path + '/src/ar3/programs/test.txt', 'r') for line in fp: line = line.strip('\n') self.add_to_queue(from_buffer=line) fp.close() ''' Jog Controls ''' def switch_jog_type(self): if self.cartesian_jog_radiobutton.isChecked(): self.jog_type = 1 keys = ['X:', 'Y:', 'Z:', 'RX:', 'RY:', 'RZ:'] for idx in range(6): self.ax_labels[idx].setText(keys[idx]) else: self.jog_type = 0 for idx in range(6): self.ax_labels[idx].setText('J' + str(idx + 1) + ":") # Cartesian X def start_jog_x_neg(self): self.set_jog_params(0, -1.0) def start_jog_x_pos(self): self.set_jog_params(0, 1.0) # Cartesian Y def start_jog_y_neg(self): self.set_jog_params(1, -1.0) def start_jog_y_pos(self): self.set_jog_params(1, 1.0) # Catesian Z def start_jog_z_neg(self): self.set_jog_params(2, -1.0) def start_jog_z_pos(self): self.set_jog_params(2, 1.0) # Cartesian RX def start_jog_rx_neg(self): self.set_jog_params(3, -1.0) def start_jog_rx_pos(self): self.set_jog_params(3, 1.0) # Cartesian RY def start_jog_ry_neg(self): self.set_jog_params(4, -1.0) def start_jog_ry_pos(self): self.set_jog_params(4, 1.0) # Cartesian RZ def start_jog_rz_neg(self): self.set_jog_params(5, -1.0) def start_jog_rz_pos(self): self.set_jog_params(5, 1.0) def set_jog_params(self, idx, ax_dir): if self.jog_type == 1: self.joint_jog_idx = idx self.joint_jog_dir = ax_dir self.starting_pose = list(self.tcp_pose) self.jog_step = 1 self.timer.start(self.jog_rate) elif self.jog_type == 0: self.joint_jog_idx = idx self.joint_jog_dir = ax_dir self.timer.start(self.jog_rate) def stop_jogging(self): self.timer.stop() self.stop_motion() def stop_motion(self): self.robot_controller.AR3Control.joint_angles = self.feedback_angles self.robot_controller.send_joints() def jog_joints(self): if self.jog_type == 1: pose = self.starting_pose quat = quaternion_from_euler(pose[3], pose[4], pose[5], 'rxyz') rx, ry, rz, rw = quat pose[self.joint_jog_idx] += (.001 * self.jog_step) * self.joint_jog_dir x, y, z, = pose[0], pose[1], pose[2] self.qinit = list(self.feedback_angles) angles = self.solver.get_ik(self.qinit, x, y, z, rx, ry, rz, rw) if not angles: print("Solution not found!") return angles = list(angles) self.jog_step += 1 else: angles = list(self.feedback_angles) angles[self.joint_jog_idx] += .2 * self.joint_jog_dir self.gripper_angle = self.robot_controller.AR3Feedback.gripper_angle self.speed = self.speed_spinbox.value() self.robot_controller.AR3Control.accelerate = 1 self.robot_controller.AR3Control.speed = self.speed self.robot_controller.AR3Control.gripper_angle = self.gripper_angle self.robot_controller.AR3Control.joint_angles = angles self.robot_controller.send_joints() ''' Program Controls ''' def program_up(self): if self.queue_list.count() == 0: return self.cursor_idx = self.queue_list.currentRow() program_line = self.queue_list.takeItem(self.cursor_idx) self.program_buffer.pop(self.cursor_idx) self.cursor_idx -= 1 if self.cursor_idx < 0: self.cursor_idx = 0 self.queue_list.insertItem(self.cursor_idx, program_line) self.program_buffer.insert(self.cursor_idx, program_line) self.queue_list.setCurrentRow(self.cursor_idx) def program_down(self): if self.queue_list.count() == 0: return count = self.queue_list.count() self.cursor_idx = self.queue_list.currentRow() program_line = self.queue_list.takeItem(self.cursor_idx) self.program_buffer.pop(self.cursor_idx) self.cursor_idx += 1 if self.cursor_idx >= count: self.cursor_idx = count - 1 self.queue_list.insertItem(self.cursor_idx, program_line) self.program_buffer.insert(self.cursor_idx, program_line) self.queue_list.setCurrentRow(self.cursor_idx) def program_remove(self): if self.queue_list.count() == 0: return self.cursor_idx = self.queue_list.currentRow() self.queue_list.takeItem(self.cursor_idx) self.program_buffer.pop(self.cursor_idx) count = self.queue_list.count() if self.cursor_idx > count - 1: self.cursor_idx = count - 1 def add_wait_to_queue(self): delay = self.program_wait_spinbox.value() program_line = '' program_line += 'Wait' program_line += ' %0.2f' % (delay) self.program_buffer.append(program_line) self.cursor_idx += 1 self.queue_list.insertItem(self.cursor_idx, program_line) self.queue_list.setCurrentRow(self.cursor_idx) print("Queueing: {}".format(program_line)) def add_to_queue(self, from_buffer=None): if from_buffer: program_line = from_buffer else: program_line = '' program_line += 'JMove' for angle in self.feedback_angles: program_line += ' %.3f' % angle program_line += ' %d' % self.robot_controller.AR3Feedback.gripper_angle program_line += ' %.2f' % self.speed_spinbox.value() self.program_buffer.append(program_line) self.cursor_idx += 1 self.queue_list.insertItem(self.cursor_idx, program_line) self.queue_list.setCurrentRow(self.cursor_idx) print("Queueing: {}".format(program_line)) def clear_queue(self): self.queue_list.clear() self.program_buffer = [] def wait_for_move(self): flag = True while flag: flag = False for idx in range(0, 6): if abs(self.setpoint_angles[idx] - self.feedback_angles[idx]) > 0.01: flag = True if abs(self.gripper_angle - self.gripper_feedback) >= 1.0: flag = True def run_queue(self): print('Running queue with ' + str(self.queue_list.count()) + ' lines.') for idx in range(self.queue_list.count()): program_line = self.queue_list.item(idx).text() tokens = program_line.split(' ') if tokens[0] == 'Wait': time.sleep(float(tokens[1])) if tokens[0] == 'JMove': angles = tokens[1:7] for n in range(0, 6): angles[n] = float(angles[n]) gripper_val = int(tokens[7]) speed = float(tokens[8]) self.setpoint_angles = angles print("Moving to: {} {} {}".format(angles, gripper_val, speed)) self.robot_controller.AR3Control.accelerate = 1 self.robot_controller.AR3Control.speed = speed self.robot_controller.AR3Control.gripper_angle = gripper_val self.robot_controller.AR3Control.joint_angles = angles self.robot_controller.send_joints() self.wait_for_move() else: print('Unrecognized command!') print("Program completed.") def start_queue(self): if self.queue_list.count() == 0: return self.queue_thread = threading.Thread(target=self.run_queue) self.queue_thread.start() ''' Move to Pose Controls ''' def pull_current_config(self): idx = 0 for spinbox in self.joint_spinboxes: spinbox.setValue(self.feedback_angles[idx]) idx += 1 (trans, rot) = self.listener.lookupTransform('/world', '/tcp', rospy.Time(0)) rot = euler_from_quaternion(rot, 'rxyz') self.tcp_pose = list(trans) + list(rot) idx = 0 for spinbox in self.pose_spinboxes: spinbox.setValue(self.tcp_pose[idx]) idx += 1 self.gripper_angle_spinbox.setValue( int(self.robot_controller.AR3Feedback.gripper_angle)) def set_jog_type(self): if self.joint_radiobutton.isChecked(): self.joint_jog_widget.show() self.pose_jog_widget.hide() if self.pose_radiobutton.isChecked(): self.pose_jog_widget.show() self.joint_jog_widget.hide() def move_gripper(self): self.gripper_angle = self.gripper_angle_spinbox.value() self.robot_controller.AR3Control.gripper_angle = self.gripper_angle self.robot_controller.send_joints() def move_to_pose(self): self.robot_controller.AR3Control.accelerate = 1 if self.joint_radiobutton.isChecked(): angles = [] for spinbox in self.joint_spinboxes: angles.append(spinbox.value()) self.gripper_angle = self.gripper_angle_spinbox.value() self.speed = self.speed_spinbox.value() self.robot_controller.AR3Control.speed = self.speed self.robot_controller.AR3Control.gripper_angle = self.gripper_angle self.robot_controller.AR3Control.joint_angles = angles self.robot_controller.send_joints() elif self.pose_radiobutton.isChecked(): coord = [] for spinbox in self.pose_spinboxes: coord.append(spinbox.value()) quat = quaternion_from_euler(coord[3], coord[4], coord[5], 'rxyz') rx, ry, rz, rw = quat x, y, z = coord[0], coord[1], coord[2] self.qinit = self.feedback_angles angles = self.solver.get_ik(self.qinit, x, y, z, rx, ry, rz, rw) print("Requested solution for: {} {} {} {} {} {}".format( x, y, z, coord[3], coord[4], coord[5])) if not angles: print("Solution not found!") return else: angles = list(angles) print("Solution found!") self.gripper_angle = self.gripper_angle_spinbox.value() self.speed = self.speed_spinbox.value() self.robot_controller.AR3Control.speed = self.speed self.robot_controller.AR3Control.gripper_angle = self.gripper_angle self.robot_controller.AR3Control.joint_angles = angles self.robot_controller.send_joints() def move_to_rest(self): self.gripper_angle = self.gripper_angle_spinbox.value() self.speed = self.speed_spinbox.value() self.robot_controller.AR3Control.speed = self.speed self.robot_controller.AR3Control.gripper_angle = self.gripper_angle self.robot_controller.AR3Control.joint_angles = [ 0.0, 0.0, 1.57, 0.0, 1.4, 0.0 ] self.robot_controller.send_joints() def move_to_home(self): self.gripper_angle = self.gripper_angle_spinbox.value() self.speed = self.speed_spinbox.value() self.robot_controller.AR3Control.speed = self.speed self.robot_controller.AR3Control.gripper_angle = self.gripper_angle self.robot_controller.AR3Control.joint_angles = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] self.robot_controller.send_joints() ''' Feedback Panel ''' def update_feedback_label(self, data): self.connected = True self.frame_3.setEnabled(True) try: idx = 0 for lcd in self.joint_lcds: lcd.display('%0.3f' % data.joint_angles[idx]) idx += 1 self.feedback_angles = data.joint_angles self.ar3_feedback = data (trans, rot) = self.listener.lookupTransform('/world', '/tcp', rospy.Time(0)) rot = euler_from_quaternion(rot, 'rxyz') self.tcp_pose = list(trans) + list(rot) idx = 0 for lcd in self.tcp_lcds: lcd.display('%0.3f' % self.tcp_pose[idx]) idx += 1 self.gripper_lcd.display(data.gripper_angle) self.gripper_feedback = data.gripper_angle if data.eStop: self.state_label.setText("E-STOP PRESSED") elif data.running: self.state_label.setText("RUNNING") except: print('Failed to update the current config panel.')
class Compensation(): def __init__(self, Fg=[90.06, -42.79, -52.23], F_thresh=10.0, K=0.015): rospy.init_node('ft_feedback_control') self.Fg = np.array(Fg) self.F_thresh = F_thresh self.K = K self.Tmax = 2.0 self.js = None self.wind_up = 0 self.IK = IK('ceiling', 'blue_robotiq_ft_frame_id', solve_type='Distance') self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander('blue_arm') self.listener = tf.TransformListener() rospy.Subscriber("/blue/joint_states", JointState, self.joint_callback) rospy.Subscriber("/robotiq_ft_wrench", WrenchStamped, self.ft_callback, queue_size=1) self.client = actionlib.SimpleActionClient( 'blue/follow_joint_trajectory', FollowJointTrajectoryAction) rospy.spin() def joint_callback(self, data): self.js = np.array(data.position) self.js_names = data.name def ft_callback(self, data): ft = np.array( [data.wrench.force.x, data.wrench.force.y, data.wrench.force.z]) self.wind_up += 1 self.wind_up = min(self.wind_up, 100) df = np.sum(np.abs(ft - self.Fg)) # print('FT dev',df,self.wind_up) if (df > self.F_thresh) and (self.wind_up > 50): if self.js is not None: self.listener.waitForTransform(self.group.get_planning_frame(), "/blue_robotiq_ft_frame_id", data.header.stamp, rospy.Duration(4.0)) p = PointStamped() p.header = data.header p.point.x = self.K * (ft - self.Fg)[0] p.point.y = self.K * (ft - self.Fg)[1] p.point.z = self.K * (ft - self.Fg)[2] pose = self.group.get_current_pose().pose print('New force deviation:') print(ft, self.Fg) print(p.point.x, p.point.y, p.point.z) pnew = self.listener.transformPoint('ceiling', p) print('Original pose:', pose.position.z, pose.position.x, pose.position.y) pose.position.z = pnew.point.z pose.position.y = pnew.point.y pose.position.x = pnew.point.x print('New pose', pose.position.z, pose.position.x, pose.position.y) sol = None retries = 0 while not sol and retries < 20: sol = self.IK.get_ik(self.js, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) retries += 1 if retries < 20: print(sol) print(self.js) print(np.sum((self.js - np.array(sol))**2)) if (np.sum((self.js - np.array(sol))**2) < 0.5): self.publish(sol) self.wind_up = 0 else: self.Fg = 0.1 * ft + 0.9 * self.Fg def publish(self, q): if q is not None: goal = FollowJointTrajectoryGoal() jtp = JointTrajectoryPoint() jtp.positions = list(q) jtp.velocities = [0.0] * len(jtp.positions) jtp.time_from_start = rospy.Time(self.Tmax) goal.trajectory.points.append(jtp) goal.trajectory.header.stamp = rospy.Time.now() goal.trajectory.joint_names = self.js_names self.client.send_goal(goal) self.client.wait_for_result()
from trajectory_msgs.msg import JointTrajectoryPoint import rospy from trac_ik_python.trac_ik import IK from tf.transformations import quaternion_from_euler ik_solver = IK("base_link", "wrist_2_link") seed_state = [0.0] * ik_solver.number_of_joints # Convert RPY to Quaternions q = quaternion_from_euler(0, 0, 0) joint_space = ik_solver.get_ik(seed_state, 0.0, 0.50, 0.3, # X, Y, Z q[0], q[1], q[2], q[3]) # QX, QY, QZ, QW print(joint_space) waypoints = [[joint_space[0], joint_space[1], joint_space[2], joint_space[3], joint_space[4], 0]] def main(): rospy.init_node('send_joints') pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header()
class Rbx1_Kinematics(object): def __init__( self ): # gets URDF from /robot_description parameter on param server rospy.init_node('trac_ik_py') rospy.loginfo("Kinematic thingy initializin' here...") self.ik_solver = IK("base_link", "Link_6") self.seed_state = [0.0] * self.ik_solver.number_of_joints self.publisher = rospy.Publisher('/joint_states', JointState, queue_size=10) self.seq = 0 self.z_offset = 0.1575 self.scale = 1 self.x = self.y = self.z = self.qx = self.qy = self.qz = self.qw = 0 # keep track of EE pose def move(self, x, y, z, qx, qy, qz, qw): print("{} {} {} {} {} {} {}".format(x, y, z, qx, qy, qz, qw)) joint_states = self.ik_solver.get_ik(self.seed_state, x, y, z, qx, qy, qz, qw) print(joint_states) if joint_states == None: raise ValueError("Can't reach target.") hdr = Header() hdr.seq = self.seq = self.seq + 1 hdr.stamp = rospy.Time.now() hdr.frame_id = "trac-ik-py" js = JointState() js.header = hdr js.name = [ "Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6" ] #, , , "Joint_Gripper" js.position = joint_states js.velocity = [] js.effort = [] self.publisher.publish(js) # since we got moved the robot, let's keep track of current EE pose self.x = x self.y = y self.z = z self.qx = qx self.qy = qy self.qz = qz self.qw = qw def callback(self, data): # print(data) self.move(data.pose.position.x + (self.x * self.scale), data.pose.position.y + (self.y * self.scale), data.pose.position.z + (self.z * self.scale), self.qx, self.qy, self.qz, self.qw) # NOTE: ignoring orientation because I want EE to stay straight down! # data.pose.orientation.x + (self.qx * self.scale), # data.pose.orientation.y + (self.qy * self.scale), # data.pose.orientation.z + (self.qz * self.scale), # data.pose.orientation.w + (self.qw * self.scale)) def listener(self): rospy.Subscriber("/unity/controllers/left", PoseStamped, self.callback) rospy.spin()
class World(object): def __init__(self, robot_name=FRANKA_CARTER, use_gui=True, full_kitchen=False): self.task = None self.interface = None self.client = connect(use_gui=use_gui) set_real_time(False) #set_caching(False) # Seems to make things worse disable_gravity() add_data_path() set_camera_pose(camera_point=[2, -1.5, 1]) if DEBUG: draw_pose(Pose(), length=3) #self.kitchen_yaml = load_yaml(KITCHEN_YAML) with HideOutput(enable=True): self.kitchen = load_pybullet(KITCHEN_PATH, fixed_base=True, cylinder=True) self.floor = load_pybullet('plane.urdf', fixed_base=True) z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2] point = np.array(get_point(self.kitchen)) - np.array([0, 0, z]) set_point(self.floor, point) self.robot_name = robot_name if self.robot_name == FRANKA_CARTER: urdf_path, yaml_path = FRANKA_CARTER_PATH, None #urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML #elif self.robot_name == EVE: # urdf_path, yaml_path = EVE_PATH, None else: raise ValueError(self.robot_name) self.robot_yaml = yaml_path if yaml_path is None else load_yaml( yaml_path) with HideOutput(enable=True): self.robot = load_pybullet(urdf_path) #dump_body(self.robot) #chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link')) #wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link')) #wait_for_user() set_point(self.robot, Point(z=stable_z(self.robot, self.floor))) self.set_initial_conf() self.gripper = create_gripper(self.robot) self.environment_bodies = {} if full_kitchen: self._initialize_environment() self._initialize_ik(urdf_path) self.initial_saver = WorldSaver() self.body_from_name = {} # self.path_from_name = {} self.names_from_type = {} self.custom_limits = {} self.base_limits_handles = [] self.cameras = {} self.disabled_collisions = set() if self.robot_name == FRANKA_CARTER: self.disabled_collisions.update( tuple(link_from_name(self.robot, link) for link in pair) for pair in DISABLED_FRANKA_COLLISIONS) self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf) #self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left')) self.calibrate_conf = FConf( self.robot, self.arm_joints, self.default_conf) # Must differ from carry_conf self.special_confs = [self.carry_conf] #, self.calibrate_conf] self.open_gq = FConf(self.robot, self.gripper_joints, get_max_limits(self.robot, self.gripper_joints)) self.closed_gq = FConf(self.robot, self.gripper_joints, get_min_limits(self.robot, self.gripper_joints)) self.gripper_confs = [self.open_gq, self.closed_gq] self.open_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.open_conf(joint)]) for joint in self.kitchen_joints } self.closed_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)]) for joint in self.kitchen_joints } self._update_custom_limits() self._update_initial() def _initialize_environment(self): # wall to fridge: 4cm # fridge to goal: 1.5cm # hitman to range: 3.5cm # range to indigo: 3.5cm self.environment_poses = read_json(POSES_PATH) root_from_world = get_link_pose(self.kitchen, self.world_link) for name, world_from_part in self.environment_poses.items(): if name in ['range']: continue visual_path = os.path.join(KITCHEN_LEFT_PATH, '{}.obj'.format(name)) collision_path = os.path.join(KITCHEN_LEFT_PATH, '{}_collision.obj'.format(name)) mesh_path = None for path in [collision_path, visual_path]: if os.path.exists(path): mesh_path = path break if mesh_path is None: continue body = load_pybullet(mesh_path, fixed_base=True) root_from_part = multiply(root_from_world, world_from_part) if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']: (pos, quat) = root_from_part # TODO: still not totally aligned pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005]) root_from_part = (pos, quat) self.environment_bodies[name] = body set_pose(body, root_from_part) # TODO: release bounding box or convex hull # TODO: static object nonconvex collisions if TABLE_NAME in self.environment_bodies: body = self.environment_bodies[TABLE_NAME] _, (w, l, _) = approximate_as_prism(body) _, _, z = get_point(body) new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z), Euler(yaw=np.pi / 2)) set_pose(body, new_pose) def _initialize_ik(self, urdf_path): if not USE_TRACK_IK: self.ik_solver = None return from trac_ik_python.trac_ik import IK # killall -9 rosmaster base_link = get_link_name( self.robot, parent_link_from_joint(self.robot, self.arm_joints[0])) tip_link = get_link_name(self.robot, child_link_from_joint(self.arm_joints[-1])) # limit effort and velocities are required # solve_type: Speed, Distance, Manipulation1, Manipulation2 # TODO: fast solver and slow solver self.ik_solver = IK(base_link=str(base_link), tip_link=str(tip_link), timeout=0.01, epsilon=1e-5, solve_type="Speed", urdf_string=read(urdf_path)) if not CONSERVITIVE_LIMITS: return lower, upper = self.ik_solver.get_joint_limits() buffer = JOINT_LIMITS_BUFFER * np.ones(len(self.ik_solver.joint_names)) lower, upper = lower + buffer, upper - buffer lower[6] = -MAX_FRANKA_JOINT7 upper[6] = +MAX_FRANKA_JOINT7 self.ik_solver.set_joint_limits(lower, upper) def _update_initial(self): # TODO: store initial poses as well? self.initial_saver = WorldSaver() self.goal_bq = FConf(self.robot, self.base_joints) self.goal_aq = FConf(self.robot, self.arm_joints) if are_confs_close(self.goal_aq, self.carry_conf): self.goal_aq = self.carry_conf self.goal_gq = FConf(self.robot, self.gripper_joints) self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq] set_all_static() def is_real(self): return (self.task is not None) and self.task.real @property def constants(self): return self.special_confs + self.gripper_confs + self.initial_confs ######################### @property def base_joints(self): return joints_from_names(self.robot, BASE_JOINTS) @property def arm_joints(self): #if self.robot_name == EVE: # return get_eve_arm_joints(self.robot, arm=DEFAULT_ARM) joint_names = ['panda_joint{}'.format(1 + i) for i in range(7)] #joint_names = self.robot_yaml['cspace'] return joints_from_names(self.robot, joint_names) @property def gripper_joints(self): #if self.robot_yaml is None: # return [] joint_names = ['panda_finger_joint{}'.format(1 + i) for i in range(2)] #joint_names = [joint_from_name(self.robot, rule['name']) # for rule in self.robot_yaml['cspace_to_urdf_rules']] return joints_from_names(self.robot, joint_names) @property def kitchen_joints(self): joint_names = get_joint_names(self.kitchen, get_movable_joints(self.kitchen)) #joint_names = self.kitchen_yaml['cspace'] #return joints_from_names(self.kitchen, joint_names) return joints_from_names(self.kitchen, filter(ALL_JOINTS.__contains__, joint_names)) @property def base_link(self): return child_link_from_joint(self.base_joints[-1]) @property def franka_link(self): return parent_link_from_joint(self.robot, self.arm_joints[0]) @property def gripper_link(self): return parent_link_from_joint(self.robot, self.gripper_joints[0]) @property def tool_link(self): return link_from_name(self.robot, get_tool_link(self.robot)) @property def world_link(self): # for kitchen return BASE_LINK @property def door_links(self): door_links = set() for joint in self.kitchen_joints: door_links.update(get_link_subtree(self.kitchen, joint)) return door_links @property def static_obstacles(self): # link=None is fine # TODO: decompose obstacles #return [(self.kitchen, frozenset(get_links(self.kitchen)) - self.door_links)] return {(self.kitchen, frozenset([link])) for link in set(get_links(self.kitchen)) - self.door_links} | \ {(body, None) for body in self.environment_bodies.values()} @property def movable(self): # movable base return set(self.body_from_name) # frozenset? @property def fixed(self): # fixed base return set(self.environment_bodies.values()) | {self.kitchen} @property def all_bodies(self): return self.movable | self.fixed | {self.robot} @property def default_conf(self): # if self.robot_name == EVE: # # Eve starts outside of joint limits # # Eve starts outside of joint limits # conf = [np.average(get_joint_limits(self.robot, joint)) for joint in self.arm_joints] # #conf = np.zeros(len(self.arm_joints)) # #conf[3] -= np.pi / 2 # return conf return DEFAULT_ARM_CONF #conf = np.array(self.robot_yaml['default_q']) ##conf[1] += np.pi / 4 ##conf[3] -= np.pi / 4 #return conf ######################### # TODO: could perform base motion planning without free joints def get_base_conf(self): return get_joint_positions(self.robot, self.base_joints) def set_base_conf(self, conf): set_joint_positions(self.robot, self.base_joints, conf) def get_base_aabb(self): franka_links = get_link_subtree(self.robot, self.franka_link) base_links = get_link_subtree(self.robot, self.base_link) return aabb_union( get_aabb(self.robot, link) for link in set(base_links) - set(franka_links)) def get_world_aabb(self): return aabb_union(get_aabb(body) for body in self.fixed) # self.all_bodies def _update_custom_limits(self, min_extent=0.0): #robot_extent = get_aabb_extent(get_aabb(self.robot)) # Scaling by 0.5 to prevent getting caught in corners #min_extent = 0.5 * min(robot_extent[:2]) * np.ones(2) / 2 world_aabb = self.get_world_aabb() full_lower, full_upper = world_aabb base_limits = (full_lower[:2] - min_extent, full_upper[:2] + min_extent) base_limits[1][0] = COMPUTER_X - min_extent # TODO: robot radius base_limits[0][1] += 0.1 base_limits[1][1] -= 0.1 for handle in self.base_limits_handles: remove_debug(handle) self.base_limits_handles = [] #self.base_limits_handles.extend(draw_aabb(world_aabb)) z = get_point(self.floor)[2] + 1e-2 if DEBUG: self.base_limits_handles.extend(draw_base_limits(base_limits, z=z)) self.custom_limits = custom_limits_from_base_limits( self.robot, base_limits) return self.custom_limits def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF): assert self.ik_solver is not None init_lower, init_upper = self.ik_solver.get_joint_limits() base_link = link_from_name(self.robot, self.ik_solver.base_link) world_from_base = get_link_pose(self.robot, base_link) tip_link = link_from_name(self.robot, self.ik_solver.tip_link) tool_from_tip = multiply( invert(get_link_pose(self.robot, self.tool_link)), get_link_pose(self.robot, tip_link)) world_from_tip = multiply(world_from_tool, tool_from_tip) base_from_tip = multiply(invert(world_from_base), world_from_tip) joints = joints_from_names( self.robot, self.ik_solver.joint_names) # self.ik_solver.link_names seed_state = get_joint_positions(self.robot, joints) # seed_state = [0.0] * self.ik_solver.number_of_joints lower, upper = init_lower, init_upper if nearby_tolerance < INF: tolerance = nearby_tolerance * np.ones(len(joints)) lower = np.maximum(lower, seed_state - tolerance) upper = np.minimum(upper, seed_state + tolerance) self.ik_solver.set_joint_limits(lower, upper) (x, y, z), (rx, ry, rz, rw) = base_from_tip # TODO: can also adjust tolerances conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw) self.ik_solver.set_joint_limits(init_lower, init_upper) if conf is None: return conf # if nearby_tolerance < INF: # print(lower.round(3)) # print(upper.round(3)) # print(conf) # print(get_difference(seed_state, conf).round(3)) set_joint_positions(self.robot, joints, conf) return get_configuration(self.robot) def solve_pybullet_ik(self, world_from_tool, nearby_tolerance): start_time = time.time() # Most of the time is spent creating the robot # TODO: use the waypoint version that doesn't repeatedly create the robot current_conf = get_joint_positions(self.robot, self.arm_joints) full_conf = sub_inverse_kinematics( self.robot, self.arm_joints[0], self.tool_link, world_from_tool, custom_limits=self.custom_limits ) # , max_iterations=1) # , **kwargs) conf = get_joint_positions(self.robot, self.arm_joints) max_distance = get_distance(current_conf, conf, norm=INF) if nearby_tolerance < max_distance: return None print('Nearby) time: {:.3f} | distance: {:.5f}'.format( elapsed_time(start_time), max_distance)) return full_conf def solve_inverse_kinematics(self, world_from_tool, nearby_tolerance=INF, **kwargs): if self.ik_solver is not None: return self.solve_trac_ik(world_from_tool, **kwargs) #if nearby_tolerance != INF: # return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance) current_conf = get_joint_positions(self.robot, self.arm_joints) start_time = time.time() if nearby_tolerance == INF: generator = ikfast_inverse_kinematics(self.robot, PANDA_INFO, self.tool_link, world_from_tool, max_attempts=10, use_halton=True) else: generator = closest_inverse_kinematics( self.robot, PANDA_INFO, self.tool_link, world_from_tool, max_time=0.01, max_distance=nearby_tolerance, use_halton=True) conf = next(generator, None) #conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100) if conf is None: return conf max_distance = get_distance(current_conf, conf, norm=INF) #print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format( # elapsed_time(start_time), max_distance, nearby_tolerance)) set_joint_positions(self.robot, self.arm_joints, conf) return get_configuration(self.robot) ######################### def set_initial_conf(self): set_joint_positions(self.robot, self.base_joints, [2.0, 0, np.pi]) #for rule in self.robot_yaml['cspace_to_urdf_rules']: # gripper: max is open # joint = joint_from_name(self.robot, rule['name']) # set_joint_position(self.robot, joint, rule['value']) set_joint_positions(self.robot, self.arm_joints, self.default_conf) # active_task_spaces # if self.robot_name == EVE: # for arm in ARMS: # joints = get_eve_arm_joints(self.robot, arm)[2:4] # set_joint_positions(self.robot, joints, -0.2*np.ones(len(joints))) def set_gripper(self, value): positions = value * np.ones(len(self.gripper_joints)) set_joint_positions(self.robot, self.gripper_joints, positions) def close_gripper(self): self.closed_gq.assign() def open_gripper(self): self.open_gq.assign() ######################### def get_door_sign(self, joint): return -1 if 'left' in get_joint_name(self.kitchen, joint) else +1 def closed_conf(self, joint): lower, upper = get_joint_limits(self.kitchen, joint) if 'drawer' in get_joint_name(self.kitchen, joint): fraction = 0.9 return fraction * lower + (1 - fraction) * upper if 'left' in get_joint_name(self.kitchen, joint): return upper return lower def open_conf(self, joint): joint_name = get_joint_name(self.kitchen, joint) if 'left' in joint_name: open_position = get_min_limit(self.kitchen, joint) else: open_position = get_max_limit(self.kitchen, joint) #print(get_joint_name(self.kitchen, joint), get_min_limit(self.kitchen, joint), get_max_limit(self.kitchen, joint)) # drawers: [0.0, 0.4] # left doors: [-1.57, 0.0] # right doors: [0.0, 1.57] if joint_name in CABINET_JOINTS: # TODO: could make fraction of max return CABINET_OPEN_ANGLE * open_position / abs(open_position) if joint_name in DRAWER_JOINTS: return DRAWER_OPEN_FRACTION * open_position return open_position def close_door(self, joint): set_joint_position(self.kitchen, joint, self.closed_conf(joint)) def open_door(self, joint): set_joint_position(self.kitchen, joint, self.open_conf(joint)) ######################### def add_camera(self, name, pose, camera_matrix, max_depth=KINECT_DEPTH, display=False): color = apply_alpha(RED, 0.1 if DEBUG else 0) cone = get_viewcone(depth=max_depth, camera_matrix=camera_matrix, color=color, mass=0, collision=False) set_pose(cone, pose) if display: kinect = load_pybullet(KINECT_URDF, fixed_base=True) set_pose(kinect, pose) set_color(kinect, BLACK) self.add(name, kinect) self.cameras[name] = Camera(cone, camera_matrix, max_depth) if DEBUG: draw_pose(pose) step_simulation() return name def get_supporting(self, obj_name): # is_placed_on_aabb | is_center_on_aabb # Only want to generate stable placements, but can operate on initially unstable ones # TODO: could filter orientation as well body = self.get_body(obj_name) supporting = { surface for surface in ALL_SURFACES if is_center_on_aabb(body, compute_surface_aabb(self, surface), above_epsilon=5e-2, below_epsilon=5e-2) } if ('range' in supporting) and (len(supporting) == 2): # TODO: small hack for now supporting -= {'range'} if len(supporting) != 1: print('{} is not supported by a single surface ({})!'.format( obj_name, supporting)) return None [surface_name] = supporting return surface_name def fix_pose(self, name, pose=None, fraction=0.5): body = self.get_body(name) if pose is None: pose = get_pose(body) else: set_pose(body, pose) # TODO: can also drop in simulation x, y, z = point_from_pose(pose) roll, pitch, yaw = euler_from_quat(quat_from_pose(pose)) quat = quat_from_euler(Euler(yaw=yaw)) set_quat(body, quat) surface_name = self.get_supporting(name) if surface_name is None: return None, None if fraction == 0: new_pose = (Point(x, y, z), quat) return new_pose, surface_name surface_aabb = compute_surface_aabb(self, surface_name) new_z = (1 - fraction) * z + fraction * stable_z_on_aabb( body, surface_aabb) point = Point(x, y, new_z) set_point(body, point) print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format( name, roll, pitch, new_z - z)) new_pose = (point, quat) return new_pose, surface_name # def fix_geometry(self): # for name in self.movable: # fixed_pose, _ = self.fix_pose(name) # if fixed_pose is not None: # set_pose(self.get_body(name), fixed_pose) ######################### def add(self, name, body): assert name not in self.body_from_name if DEBUG: add_body_name(body, name) self.body_from_name[name] = body return name def add_body(self, name, **kwargs): obj_type = type_from_name(name) self.names_from_type.setdefault(obj_type, []).append(name) path = get_obj_path(obj_type) #self.path_from_name[name] = path print('Loading', path) body = load_pybullet(path, **kwargs) assert body is not None self.add(name, body) def get_body(self, name): return self.body_from_name[name] # def get_body_path(self, name): # return self.path_from_name[name] # def get_body_type(self, name): # filename, _ = os.path.splitext(os.path.basename(self.get_body_path(name))) # return filename def get_name(self, name): inverse = {v: k for k, v in self.body_from_name.items()} return inverse.get(name, None) def remove_body(self, name): body = self.get_body(name) remove_body(body) del self.body_from_name[name] def reset(self): #remove_all_debug() for camera in self.cameras.values(): remove_body(camera.body) #remove_body(camera.kinect) self.cameras = {} for name in list(self.body_from_name): self.remove_body(name) def destroy(self): reset_simulation() disconnect()
class Arm(object): """ UR3 arm controller """ def __init__(self, ft_sensor=False, driver=ROBOT_GAZEBO, ee_transform=None, robot_urdf='ur3e_robot', ik_solver=IKFAST, namespace='ur3', gripper=False, ft300=False): """ ft_sensor bool: whether or not to try to load ft sensor information driver string: type of driver to use for real robot or simulation. supported: gazebo, ur_modern_driver, ur_rtde_driver (Newest) use ur_control.constants e.g. ROBOT_GAZEBO ee_tranform array [x,y,z,ax,ay,az,w]: optional transformation to the end-effector that is applied before doing any operation in task-space robot_urdf string: name of the robot urdf file to be used namespace string: nodes namespace prefix gripper bool: enable gripper control """ cprint.ok("ft_sensor: {}, driver: {}, ee_transform: {}, \n robot_urdf: {}".format(ft_sensor, driver, ee_transform, robot_urdf)) self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._current_ft = [] self.ft_sensor = None self.ft300 = ft300 self.ik_solver = ik_solver self.ee_transform = ee_transform self.ns = namespace self.ee_link = 'tool0' self.max_joint_speed = np.deg2rad([100, 100, 100, 200, 200, 200]) # self.max_joint_speed = np.deg2rad([191, 191, 191, 371, 371, 371]) self._init_ik_solver(robot_urdf) self._init_controllers(driver, gripper) if ft_sensor: self._init_ft_sensor(driver) ### private methods ### def _init_controllers(self, driver, gripper): traj_publisher = None namespace = '' if driver == ROBOT_UR_MODERN_DRIVER: traj_publisher = JOINT_PUBLISHER_REAL elif driver == ROBOT_UR_RTDE_DRIVER: traj_publisher = JOINT_PUBLISHER_BETA elif driver == ROBOT_GAZEBO: traj_publisher = JOINT_PUBLISHER_SIM elif driver == ROBOT_GAZEBO_DUAL_LEFT: traj_publisher = JOINT_PUBLISHER_SIM namespace = JOINT_PUBLISHER_SIM_DUAL_LEFT elif driver == ROBOT_GAZEBO_DUAL_RIGHT: traj_publisher = JOINT_PUBLISHER_SIM namespace = JOINT_PUBLISHER_SIM_DUAL_RIGHT else: raise Exception("unsupported driver", driver) # Flexible trajectory (point by point) traj_publisher_flex = '/' + namespace + traj_publisher + '/command' print(traj_publisher_flex) cprint.blue("connecting to: {}".format(traj_publisher)) self._flex_trajectory_pub = rospy.Publisher(traj_publisher_flex, JointTrajectory, queue_size=10) self.joint_traj_controller = JointTrajectoryController( publisher_name=traj_publisher, namespace=namespace) self.gripper = None if gripper: self.gripper = GripperController(namespace=namespace, timeout=2.0) def _init_ik_solver(self, robot_urdf): self.kdl = ur_kinematics(robot_urdf, ee_link=self.ee_link) if self.ik_solver == IKFAST: # IKfast libraries if robot_urdf == 'ur3_robot': self.arm_ikfast = ur_ikfast.URKinematics('ur3') elif robot_urdf == 'ur3e_robot': self.arm_ikfast = ur_ikfast.URKinematics('ur3e') elif self.ik_solver == TRAC_IK: self.trac_ik = IK(base_link="base_link", tip_link=self.ee_link, timeout=0.001, epsilon=1e-5, solve_type="Speed", urdf_string=utils.load_urdf_string('ur_pykdl', robot_urdf)) else: raise Exception("unsupported ik_solver", self.ik_solver) def _init_ft_sensor(self, driver): # Publisher of wrench self.pub_ee_wrench = rospy.Publisher('/%s/ee_ft' % self.ns, Wrench, queue_size=50) self.pub_raw_ft = rospy.Publisher('/%s/filtered_ft' % self.ns, Wrench, queue_size=50) if driver == ROBOT_GAZEBO: self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_SIM) else: if self.ft300: self.ft_sensor = FTsensor(namespace=FT300_SUBSCRIBER_REAL) else: self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_REAL) rospy.sleep(1) self.set_wrench_offset(override=False) def _update_wrench_offset(self): self.wrench_offset = self.get_filtered_ft().tolist() rospy.set_param('/%s/ft_offset' % self.ns, self.wrench_offset) def _flexible_trajectory(self, position, time=5.0, vel=None): """ Publish point by point making it more flexible for real-time control """ # Set up a trajectory message to publish. action_msg = JointTrajectory() action_msg.joint_names = JOINT_ORDER # Create a point to tell the robot to move to. target = JointTrajectoryPoint() target.positions = position # These times determine the speed at which the robot moves: if vel is not None: target.velocities = [vel] * 6 target.time_from_start = rospy.Duration(time) # Package the single point into a trajectory of points with length 1. action_msg.points = [target] self._flex_trajectory_pub.publish(action_msg) def _solve_ik(self, pose): if self.ee_transform is not None: inv_ee_transform = np.copy(self.ee_transform) inv_ee_transform[:3] *= -1 pose = np.array(conversions.transform_end_effector(pose, inv_ee_transform)) if self.ik_solver == IKFAST: ik = self.arm_ikfast.inverse(pose, q_guess=self.joint_angles()) elif self.ik_solver == TRAC_IK: ik = self.trac_ik.get_ik(self.joint_angles(), pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]) if ik is None: print("IK not found") return ik ### Public methods ### def get_filtered_ft(self): """ Get measurements from FT Sensor. Measurements are filtered with a low-pass filter. Measurements are given in sensors orientation. """ if self.ft_sensor is None: raise Exception("FT Sensor not initialized") ft_limitter = [300, 300, 300, 30, 30, 30] # Enforce measurement limits (simulation) ft = self.ft_sensor.get_filtered_wrench() ft = [ ft[i] if abs(ft[i]) < ft_limitter[i] else ft_limitter[i] for i in range(6) ] return np.array(ft) def set_wrench_offset(self, override=False): """ Set a wrench offset """ if override: self._update_wrench_offset() else: self.wrench_offset = rospy.get_param('/%s/ft_offset' % self.ns, None) if self.wrench_offset is None: self._update_wrench_offset() def get_ee_wrench_hist(self, hist_size=24): if self.ft_sensor is None: raise Exception("FT Sensor not initialized") q_hist = self.joint_traj_controller.get_joint_positions_hist()[:hist_size] ft_hist = self.ft_sensor.get_filtered_wrench(hist_size=hist_size) if self.wrench_offset is not None: ft_hist = np.array(ft_hist) - np.array( self.wrench_offset) poses_hist = [self.end_effector(q) for q in q_hist] wrench_hist = [spalg.convert_wrench(wft, p).tolist() for p, wft in zip(poses_hist, ft_hist)] return np.array(wrench_hist) def get_ee_wrench(self): """ Compute the wrench (force/torque) in task-space """ if self.ft_sensor is None: return np.zeros(6) wrench_force = self.ft_sensor.get_filtered_wrench() if self.wrench_offset is not None: wrench_force = np.array(wrench_force) - np.array( self.wrench_offset) # compute force transformation? # # # Transform of EE pose = self.end_effector() return spalg.convert_wrench(wrench_force, pose) def publish_wrench(self): if self.ft_sensor is None: raise Exception("FT Sensor not initialized") " Publish arm's end-effector wrench " wrench = self.get_ee_wrench() # Note you need to call rospy.init_node() before this will work self.pub_ee_wrench.publish(conversions.to_wrench(wrench)) def publish_ft_raw(self): if self.ft_sensor is None: raise Exception("FT Sensor not initialized") " Publish arm's end-effector wrench " wrench_force = self.ft_sensor.get_filtered_wrench() if self.wrench_offset is not None: wrench_force = np.array(wrench_force) - np.array( self.wrench_offset) # Note you need to call rospy.init_node() before this will work self.pub_raw_ft.publish(conversions.to_wrench(wrench_force)) def end_effector(self, joint_angles=None, rot_type='quaternion'): """ Return End Effector Pose """ joint_angles = self.joint_angles() if joint_angles is None else joint_angles if rot_type == 'quaternion': # forward kinematics if self.ik_solver == IKFAST: x = self.arm_ikfast.forward(joint_angles) else: x = self.kdl.forward_position_kinematics(joint_angles) # apply extra transformation of end-effector if self.ee_transform is not None: x = np.array(conversions.transform_end_effector(x, self.ee_transform)) return x elif rot_type == 'euler': x = self.end_effector(joint_angles) euler = np.array(transformations.euler_from_quaternion(x[3:], axes='rxyz')) return np.concatenate((x[:3], euler)) else: raise Exception("Rotation Type not supported", rot_type) def joint_angle(self, joint): """ Return the requested joint angle. @type joint: str @param joint: name of a joint @rtype: float @return: angle in radians of individual joint """ return self.joint_traj_controller.get_joint_positions()[joint] def joint_angles(self): """ Return all joint angles. @rtype: dict({str:float}) @return: unordered dict of joint name Keys to angle (rad) Values """ return self.joint_traj_controller.get_joint_positions() def joint_velocity(self, joint): """ Return the requested joint velocity. @type joint: str @param joint: name of a joint @rtype: float @return: velocity in radians/s of individual joint """ return self.joint_traj_controller.get_joint_velocities()[joint] def joint_velocities(self): """ Return all joint velocities. @rtype: dict({str:float}) @return: unordered dict of joint name Keys to velocity (rad/s) Values """ return self.joint_traj_controller.get_joint_velocities() ### Basic Control Methods ### def set_joint_positions(self, position, velocities=None, accelerations=None, wait=False, t=5.0): self.joint_traj_controller.add_point(positions=position, time=t, velocities=velocities, accelerations=accelerations) self.joint_traj_controller.start(delay=0.01, wait=wait) self.joint_traj_controller.clear_points() def set_joint_positions_flex(self, position, t=5.0, v=None): qc = self.joint_angles() deltaq = (qc - position) speed = deltaq / t cmd = position if np.any(np.abs(speed) > (self.max_joint_speed/t)): print("exceeded max speed", speed) return SPEED_LIMIT_EXCEEDED self._flexible_trajectory(cmd, t, v) return DONE def set_target_pose(self, pose, wait=False, t=5.0): """ Supported pose is only x y z aw ax ay az """ q = self._solve_ik(pose) if q is None: # IK not found return IK_NOT_FOUND else: return self.set_joint_positions(q, wait=wait, t=t) def set_target_pose_flex(self, pose, t=5.0): """ Supported pose is only x y z aw ax ay az """ q = self._solve_ik(pose) if q is None: # IK not found return IK_NOT_FOUND else: return self.set_joint_positions_flex(q, t=t)