max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-03-09 15:53:04 +00:00
parent 2847da69d1
commit 787bb62378
1 changed files with 32 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
new_main.py
View File

@ -69,43 +69,40 @@ class PickAndPlace(object): #class that handles moving the robot, gripper and IK
'''Function to send limb to predefined joint angles without running IK service''' '''Function to send limb to predefined joint angles without running IK service'''
self._guarded_move_to_joint_position(angles) self._guarded_move_to_joint_position(angles)
def ik_request(self, pose): def ik_request(self, pose):
'''Function to generate joint angles from target pose, does not take into account obstacles that are not the robot''' '''Function to generate joint angles from target pose, does not take into account obstacles that are not the robot'''
hdr = Header(stamp=rospy.Time.now(), frame_id='base') #This function is only used wile in te simulation environment and only for generating valid joint angles, due to te random nature of te solver, it it not 'trusted' to run in real time hdr = Header(stamp=rospy.Time.now(), frame_id='base') #This function is only used wile in te simulation environment and only for generating valid joint angles, due to te random nature of te solver, it it not 'trusted' to run in real time
ikreq = SolvePositionIKRequest() ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
try: try:
resp = self._iksvc(ikreq) resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e: except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,)) rospy.logerr("Service call failed: %s" % (e,))
return False return False
# Check if result valid, and type of seed ultimately used to get solution # Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's # convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type) resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type)
limb_joints = {} limb_joints = {}
if (resp_seeds[0] != resp.RESULT_INVALID): if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = { seed_str = {
ikreq.SEED_USER: 'User Provided Seed', ikreq.SEED_USER: 'User Provided Seed',
ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints', ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None') }.get(resp_seeds[0], 'None')
# Format solution into Limb API-compatible dictionary # Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
else: else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False return False
print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
print() print()
print() print()
print('Linb Joints:') print('Linb Joints:')
print(limb_joints) print(limb_joints)
print() print()
print() print()
print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') #Visual statemet to easily find and copy calculated joint angles print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') #Visual statemet to easily find and copy calculated joint angles
return limb_joints return limb_joints
def gripperPosition(self): def gripperPosition(self):