def calculate_mk2_ik(self): #goal_point = Point(0.298, -0.249, -0.890) home position goal_pose = Pose() # Goto position 1 goal_point = Point(0.298, -0.249, -0.890) #goal_point = Point(0.298, -0.5, -1.0) goal_pose.position = goal_point quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw goal_pose.orientation = Quaternion(*quat.tolist()) moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result() # Goto position 2 goal_point = Point(0.305, -0.249, -1.05) goal_pose.position = goal_point quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw goal_pose.orientation = Quaternion(*quat.tolist()) moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result()
def move_to_box(objcolorl): if objcolorl == 0: #move to green box pose = Pose() pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00) pose.position = Point(0.570, -0.176, 0.283) path = '/home/ynazon1/Pictures/green_success.png' img = cv2.imread(path) msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) pubpic.publish(msg) # Sleep to allow for image to be published. rospy.sleep(1) else: #move to blue box pose = Pose() pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00) pose.position = Point(0.708, -0.153, 0.258) path = '/home/ynazon1/Pictures/red_success.png' img = cv2.imread(path) msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) pubpic.publish(msg) # Sleep to allow for image to be published. rospy.sleep(1) request_pose(pose, "left", left_group)
def pose_at_distance2(pose1, pose2, distance): """ Returns a pose that has the same orientation as the original but the position is at a distance from the original. Very usefull when you want to mantain a distance from an object. It takes into account where the robot is """ new_pose2 = Pose() new_pose2.position = substract_vector(pose2.position, pose1.position) new_pose2 = pose_at_distance(new_pose2, distance) new_pose2.position = add_vectors(new_pose2.position, pose1.position) return new_pose2
def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.makeGraspIM(pose) self.server.applyChanges()
def find_joint_pose(self, pose, targ_x=0.0, targ_y=0.0, targ_z=0.0, targ_ox=0.0, targ_oy=0.0, targ_oz=0.0): ''' Finds the joint position of the arm given some pose and the offsets from it (to avoid opening the structure all the time outside of the function). ''' ik_srv = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK) ik_request = SolvePositionIKRequest() the_pose = deepcopy(pose) the_pose['position'] = Point(x=targ_x + self.baxter_off.linear.x, y=targ_y + self.baxter_off.linear.y, z=targ_z + self.baxter_off.linear.z) angles = tf.transformations.quaternion_from_euler( \ targ_ox + self.baxter_off.angular.x, \ targ_oy + self.baxter_off.angular.y, \ targ_oz + self.baxter_off.angular.z) the_pose['orientation'] = Quaternion(x=angles[0], y=angles[1], z=angles[2], w=angles[3]) approach_pose = Pose() approach_pose.position = the_pose['position'] approach_pose.orientation = the_pose['orientation'] hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose_req = PoseStamped(header=hdr, pose=approach_pose) ik_request.pose_stamp.append(pose_req) resp = iksvc(ik_request) return dict(zip(resp.joints[0].name, resp.joints[0].position))
def main(argv): x = float(sys.argv[1]) y = float(sys.argv[2]) th = float(sys.argv[3]) print("x: %.3f y: %.3f Th: %.1f") % (x, y, th) # print 2D pose data to terminal time.sleep(0.5) #publish pose data to ros topic msg = Pose() q = tf.transformations.quaternion_from_euler(0, 0, th) msg.position = Point(x,y,0) msg.orientation.x = q[0] msg.orientation.y = q[1] msg.orientation.z = q[2] msg.orientation.w = q[3] pose_pub.publish(msg) key_pressed = False while key_pressed == False: key_pressed = select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) # is key pressed? time.sleep(0.5) sys.exit()
def project(self, p, radius, width, height) : #print p #print width #print height #print "radius" #print radius xFOV = 63.38 yFOV = 48.25 cx = width /2 cy = height /2 fx = cx / np.tan((xFOV/2) * np.pi / 180) fy = cy / np.tan((yFOV/2) * np.pi / 180) toball = np.zeros(3) toball[0] = (p[0] - cx) / fx toball[1] = -(p[1] - cy) / fy toball[2] = 1 toball = toball / np.linalg.norm(toball) #normalize so we can then multiply by distance distance = self.pixel_radius / radius toball = toball * distance pose = Pose() pose.position = Point(toball[0], toball[1], toball[2]) pose.orientation = Quaternion(0,0,0,1) #print "FOUND ORANGE BALL!!!!" #print toball return pose
def publishSphere2(self, pose, color, scale, lifetime=None): """ Publish a sphere Marker. This renders a smoother, flatter-looking sphere. @param pose (numpy matrix, numpy ndarray, ROS Pose) @param color name (string) or RGB color value (tuple or list) @param scale (ROS Vector3, float) @param lifetime (float, None = never expire) """ if (self.muted == True): return True # Convert input pose to a ROS Pose Msg if (type(pose) == numpy.matrix) or (type(pose) == numpy.ndarray): sphere_pose = mat_to_pose(pose) elif type(pose) == Pose: sphere_pose = pose elif type(pose) == Point: pose_msg = Pose() pose_msg.position = pose sphere_pose = pose_msg else: rospy.logerr("Pose is unsupported type '%s' in publishSphere()", type(pose).__name__) return False # Convert input scale to a ROS Vector3 Msg if type(scale) == Vector3: sphere_scale = scale elif type(scale) == float: sphere_scale = Vector3(scale, scale, scale) else: rospy.logerr("Scale is unsupported type '%s' in publishSphere()", type(scale).__name__) return False # Increment the ID number self.sphere_marker.id += 1 # Get the default parameters sphere_marker = self.sphere_marker2 # sphere_marker2 = SPHERE_LIST if lifetime == None: sphere_marker.lifetime = rospy.Duration(0.0) # 0 = Marker never expires else: sphere_marker.lifetime = rospy.Duration(lifetime) # in seconds # Set the timestamp sphere_marker.header.stamp = rospy.Time.now() # Set marker size sphere_marker.scale = sphere_scale # Set marker color sphere_marker.color = self.getColor(color) # Set the pose of one sphere in the list sphere_marker.points[0] = sphere_pose.position sphere_marker.colors[0] = self.getColor(color) return self.publishMarker(sphere_marker)
def __init__(self, nodes) : self.node_zone = MarkerArray() for node in nodes.nodes : marker = Marker() marker.header.frame_id = "/map" marker.type = marker.LINE_STRIP marker.scale.x = 0.1 marker.color.a = 0.8 marker.color.r = 0.7 marker.color.g = 0.1 marker.color.b = 0.2 node._get_coords() count=0 for i in node.vertices : #print i[0], i[1] Vert = Point() Vert.z = 0.05 Vert.x = node.px + i[0] Vert.y = node.py + i[1] marker.points.append(Vert) #vertname = "%s-%d" %(node.name, count) Pos = Pose() Pos.position = Vert # OJO: esto hay que hacerlo de alguna forma #self._vertex_marker(vertname, Pos, vertname) count+=1 marker.points.append(marker.points[0]) self.node_zone.markers.append(marker) idn = 0 for m in self.node_zone.markers: m.id = idn idn += 1
def callback_Distance(data): #print data global i_distance m = marker_array.markers[markers_list[id_]] m.header.stamp = rospy.Time.now() m.action = 0 m.type = m.SPHERE p = Pose() #print data.data[0], data.data[1], data.data[2] p.position = Point(data.data[0], data.data[1], data.data[2]) if data.data[3] == data.data[4] : scale = data.data[3] else : i_distance = i_distance + 1 scale = data.data[3] if i_distance%2 == 0: scale = data.data[4] m.pose = p m.scale.x = scale * 2 m.scale.y = scale * 2 m.scale.z = scale * 2 m.color.r = 0 m.color.g = 1 m.color.b = 0 m.color.a = 0.2 m.lifetime = rospy.Duration(0) m.frame_locked = False
def detect_faces_cb_ros_pkg(userdata, status, result): '''This code detects faces using the ROS package and then checks the list of people who are not able to walk. If the distance of people who are not able to walk is close to the detected faces they are popped from the list. First perosn who is closer than ---closestPersonDistance--- are sent to userdata''' LOCATION_LIST_DISTANCE = 1 print "CallBack is called" people = result.face_positions # pose_in_map = transform_pose(people.pos, people.header.frame_id, "/map") if userdata.location_list: for person in people: pose_in_stereo = Pose() pose_in_stereo.position = person.pos pose_in_map = transform_pose(pose_in_stereo, userdata.message.header.frame_id, "/map") for not_walking in userdata.location_list: distance_from_list = ((pose_in_map.position.x - not_walking[0])**2 + (pose_in_map.position.pos.y - not_walking[1])**2) if distance_from_list < LOCATION_LIST_DISTANCE: people.pop(people.index(person)) closestPerson = None closestPersonDistance = sys.maxint for person in people: dist = person.pos.z if dist < closestPersonDistance: closestPerson = person closestPersonDistance = dist # FIXME: blacklist already seen people userdata.closest_person = closestPerson print "Closest person (from find_person_with_visit_checker): ", closestPerson return succeeded if closestPerson else aborted
def get_grasp_pose(g_decision_making_state, object_grasp_pose, object_grasp_pose_set): # load pre-computer grasps # loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' ) # loaded_grasp = loaded_file['grasp'][0] # print len(loaded_grasp[g_decision_making_state.object_name]) loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' ) loaded_grasp = loaded_file['grasp'][0] num_of_grasp = len(loaded_grasp[g_decision_making_state.object_name]) temp = object_grasp_pose for i in range(0,num_of_grasp): print i print_angles(loaded_grasp[g_decision_making_state.object_name][i]) print_angles(temp) object_grasp_pose_temp = temp print_angles(temp) object_grasp_pose_set[i] = Pose() object_grasp_pose_set[i].position = temp.position object_grasp_pose_set[i].orientation = rotate_orientation(object_grasp_pose_temp.orientation, loaded_grasp[g_decision_making_state.object_name][i].orientation) move_ok = plan_and_move_end_effector_to(object_grasp_pose, 0.3, g_bin_positions[g_decision_making_state.bin_id]['map_robot_position_2_ik_seed']) print_angles(object_grasp_pose_set[i]) # raw_input("press Enter to continue...") return object_grasp_pose_set
def publisher(paths): publisher = rospy.Publisher('pathfinder', Path, queue_size=20) rospy.init_node('path_publisher', anonymous=True) rate = rospy.Rate(10) # 10 hertz msgs=[] #list for all the messages we need to publish for path in paths: msg = Path() #new path message msg.header = create_std_h() #add a header to the path message for x,y,z in path: #create a pose for each position in the path #position is the only field we care about, leave the rest as 0 ps = PoseStamped() ps.header=create_std_h() i=Pose() i.position=Point(x,y,z) ps.pose=i msg.poses.append(ps) msgs.append(msg) while not rospy.is_shutdown(): # while we're going #publish each message for m in msgs: #rospy.loginfo(m) publisher.publish(m) rate.sleep()
def obj_response_cb(userdata, response): if response.exists: pose = Pose() #Wont there be problems mixing float32 with 64? TODO pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0) #TODO, this may give problems pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z)) if MANTAIN_DISTANCE_FROM_OBJECT: pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT) userdata.object_location = pose release_pose = ObjectPose() release_pose.pose = response.arm_coordinates userdata.object_release_location = release_pose print "\n\n printing pose for move to object" print pose print "\n\n printing pose for releasing the object" print release_pose print "\n that was the pose of the object\n\n" print userdata.object_name print "is the object name" return succeeded else: userdata.object_location = None return aborted
def execute(self, userdata): userdata.orders = [DrinkOrder('david', 'coke'), DrinkOrder('michael', 'milk')] pose = Pose() pose.position = Point(0.059, 6.26, 0.0) pose.orientation = Quaternion(0, 0, -0.59, 0.8) userdata.guests_location = pose return succeeded
def transform_pose(pose_in, transform): ''' Transforms a pose Takes a pose defined in the frame defined by transform (i.e. the frame in which transform is the origin) and returns it in the frame in which transform is defined. Calling this with the origin pose will return transform. This returns pose.point = transform_point(pose_in.point, transform) pose.orientation = transform_orientation(pose_in.orientation, transform) **Args:** **pose (geometry_msgs.msg.Pose):** Pose to transform **transform (geometry_msgs.msg.Pose):** The transform **Returns:** A geometry_msgs.msg.Pose ''' pose = Pose() pose.position = transform_point(pose_in.position, transform) pose.orientation = transform_quaternion(pose_in.orientation, transform) return pose
def eef_pose_pub(): rospy.init_node('eef_publisher') rospy.loginfo("Publishing right EEF gripper location") listener = tf.TransformListener() pub = rospy.Publisher('eef_pose', Pose, queue_size=10) DEFAULT_LINK = '/right_ee_link' DEFAULT_RATE = 100 # Pull from param server the hz and EEF link eef_link = rospy.get_param("~eef_link", DEFAULT_LINK) publish_rate = rospy.get_param("~eef_rate", DEFAULT_RATE) rate = rospy.Rate(publish_rate) while not rospy.is_shutdown(): try: trans, rot = listener.lookupTransform('/base_link', eef_link, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue msg = Pose() msg.position = Point() msg.position.x, msg.position.y, msg.position.z = trans[0], trans[1], trans[2] msg.orientation = Quaternion() msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w = rot[0], rot[1], rot[2], rot[3] pub.publish(msg) rate.sleep()
def homogeneous_product_pose_transform(pose, transform): """ pose should be Pose() msg transform is Transform() """ # 1. Get the transform homogenous matrix. Ht = tf.transformations.quaternion_matrix([transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w]) Tt = [transform.translation.x, transform.translation.y, transform.translation.z] Ht[:3,3] = Tt # 2.Original pose to homogenous matrix H0 = tf.transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) T0 = [pose.position.x, pose.position.y, pose.position.z] H0[:3,3] = T0 H1 = numpy.dot(H0, Ht) # Get results from matrix result_position = H1[:3,3].T result_position = geometry_msgs.msg.Vector3(result_position[0], result_position[1], result_position[2]) result_quat = tf.transformations.quaternion_from_matrix(H1) result_quat = geometry_msgs.msg.Quaternion(result_quat[0], result_quat[1], result_quat[2], result_quat[3]) result_pose = Pose() result_pose.position = result_position result_pose.orientation = result_quat return result_pose
def _create_vertices_array(self) : for node in self.topo_map.nodes : marker = Marker() marker.header.frame_id = "/map" marker.type = marker.LINE_STRIP #marker.lifetime=2 marker.scale.x = 0.1 marker.color.a = 0.8 marker.color.r = 0.7 marker.color.g = 0.1 marker.color.b = 0.2 node._get_coords() count=0 for i in node.vertices : #print i[0], i[1] Vert = Point() Vert.z = 0.05 Vert.x = node.px + i[0] Vert.y = node.py + i[1] marker.points.append(Vert) vertname = "%s-%d" %(node.name, count) Pos = Pose() Pos.position = Vert self._vertex_marker(vertname, Pos, vertname) count+=1 marker.points.append(marker.points[0]) self.node_zone.markers.append(marker) idn = 0 for m in self.node_zone.markers: m.id = idn idn += 1
def __init__(self): smach.StateMachine.__init__(self, [succeeded, preempted, aborted], output_keys=['object_data']) with self: pose = Pose() pose.position = Point(0, 0, 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 45)) smach.StateMachine.add( 'N2_go_to_dep_policy', MoveActionState("/base_link", pose=pose), transitions = {succeeded: 'look_for_objects'}) def moped_enable_cb(userdata, response): if response.correct != None: rospy.loginfo("ENABLE_CLOSE_OBJECT_SEARCH response: " + \ str(response.correct)) return succeeded else: return aborted smach.StateMachine.add( 'ENABLE_CLOSE_OBJECT_SEARCH', ServiceState('/iri_moped_handler/enable', enable, response_cb = moped_enable_cb, #request_key = 'object_name', request = True), transitions = {succeeded:'look_for_objects'}) # FIXME: why is there a smaller ctimeout & retry on aborted? # If something goes wrong it'd rather have it fail than having # it keep hanging on forever... --Siegfried smach.StateMachine.add( 'look_for_objects', TopicReaderState( topic_name='/iri_moped_handler/outputOPL', msg_type=ObjectPoseList, timeout=10), remapping= {'message' : 'object_data'}, transitions = {succeeded: 'DISABLE_CLOSE_OBJECT_SEARCH', aborted: 'look_for_objects'}) def moped_disable_cb(userdata, response): if response.correct != None: rospy.loginfo("DISABLE_CLOSE_OBJECT_SEARCH response: " + \ str(response.correct)) return succeeded else: return aborted smach.StateMachine.add( 'DISABLE_CLOSE_OBJECT_SEARCH', ServiceState('/iri_moped_handler/enable', enable, response_cb = moped_disable_cb, #request_key = 'object_name', request = False), transitions = {succeeded: succeeded})
def move_to_object(xposl, yposl, zposl, objcolorl): global grip_force pose = Pose() pose.position = Point(xposl-0.01, yposl, 0.00) pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00) request_pose(pose, "left", left_group) rospy.sleep(0.5) # Get left hand range state dist = baxter_interface.analog_io.AnalogIO('left_hand_range').state() rospy.sleep(1) if dist > 65000: print "Out of Range" truez = -0.13 else: print "DISTANCE %f" % dist truez = dist/1000 truez = truez - 0.06 truez = - (truez) print truez poset = Pose() poset.position = Point(xposl, yposl, truez) poset.orientation = Quaternion(1.00, 0.0, 0.00, 0.00) request_pose(poset, "left", left_group) left_gripper.close() rospy.sleep(0.5) if grip_force == 0: left_gripper.open() move_to_vision() else: pose = Pose() pose.position = Point(xposl-0.01, yposl+0.01, 0.150) pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00) request_pose(pose, "left", left_group) if grip_force == 0: left_gripper.open() move_to_vision() return move_to_box(objcolorl) left_gripper.open() move_to_vision()
def move_to_vision(): # Set pose pose = Pose() pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00) pose.position = Point(0.712, 0.316, 0.250) # Request service request_pose(pose,"left", left_group)
def callback(data): #rospy.loginfo("kinect x: %f, y %f, z: %f", data.pose.position.x, data.pose.position.y, data.pose.orientation.z) pos = Pose() pos.position = data.pose.position #pub = rospy.Publisher('PosArrayFovis', PoseArray) pos.orientation = data.pose.orientation posArr.poses.append(pos) pub.publish(posArr)
def getPose(self): p=self.endpoint_pose() if len(p.keys())==0: rospy.logerr("ERROR: Pose is empty, you may want to wait a bit before calling getPose to populate data") return None pose=Pose() pose.position=Point(*p["position"]) pose.orientation=Quaternion(*p["orientation"]) return pose
def getMsg(): pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix) pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped) #Initialize the RTKLIB ROS node rospy.init_node('rtklib_messages', anonymous=True) #Define the publishing frequency of the node rate = rospy.Rate(10) #Create a socket sock = socket.socket() #Get the address of the local host host = socket.gethostname() #Connect to the RTKRCV server that is bound to port xxxx port = 5801 sock.connect((host,port)) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() ecef_stampedPose = PoseStamped() ecef_stampedPose = navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sock.recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz ecef_stampedPose.pose = ecef_pose pub_navsat.publish(navsat) pub_pose.publish(ecef_stampedPose) rate.sleep()
def init(): #Wake up Baxter baxter_interface.RobotEnable().enable() rospy.sleep(0.25) print "Baxter is enabled" print "Intitializing clients for services" global ik_service_left ik_service_left = rospy.ServiceProxy( "ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) global ik_service_right ik_service_right = rospy.ServiceProxy( "ExternalTools/right/PositionKinematicsNode/IKService", SolvePositionIK) global obj_loc_service obj_loc_service = rospy.ServiceProxy( "object_location_service", ObjLocation) global stopflag stopflag = False #Taken from the MoveIt Tutorials moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() global scene scene = moveit_commander.PlanningSceneInterface() #Activate Left Arm to be used with MoveIt global left_group left_group = MoveGroupCommander("left_arm") left_group.set_goal_position_tolerance(0.01) left_group.set_goal_orientation_tolerance(0.01) global right_group right_group = MoveGroupCommander("right_arm") pose_right = Pose() pose_right.position = Point(0.793, -0.586, 0.329) pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0) request_pose(pose_right, "right", right_group) global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() print left_gripper.parameters() global end_effector_subs end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback) rospy.sleep(1) global pubpic pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) rospy.sleep(1)
def get_pose(self): """ Get the pose from the local_data and return a ROS geometry_msgs.Pose """ pose = Pose() pose.position = get_position(self) pose.orientation = get_orientation(self) return pose
def generatePath(cmap, start, goal, parents): global path_pub global way_pub global curmap #Create GridCells() message to display path and waypoints in rviz path = GridCells() path.cell_width = cmap.info.resolution path.cell_height = cmap.info.resolution path.header.frame_id = 'map' way = GridCells() way.cell_width = cmap.info.resolution way.cell_height = cmap.info.resolution way.header.frame_id = 'map' waycells = [mapToWorldPos(cmap, goal)] #Create a list of cells starting with the start position cells = [mapToWorldPos(cmap, start)] #trace path from goal back to start current = parents[normalize(goal, cmap.info.width)] lastpt = goal last_ang = math.atan2((lastpt.y-current.y),(lastpt.x-current.x)) p = Pose() p.position = mapToWorldPos(cmap, goal) p.orientation.z = 0 ret = [p] while current != start: cells.append(mapToWorldPos(cmap, current)) #if we change travel direction, add a waypoint ang = math.atan2((lastpt.y-current.y),(lastpt.x-current.x)) if (abs(ang-last_ang) > 0.1): waycells.append(mapToWorldPos(cmap, lastpt)) p.position = mapToWorldPos(cmap, lastpt) p.orientation.z = math.sin(ang/2) ret.append(p) last_ang = ang lastpt = current current = parents[normalize(current, cmap.info.width)] path.cells = cells way.cells = list(reversed(waycells)) path_pub.publish(path) way_pub.publish(way) resp = GetPathResponse() resp.path = ret return resp
def __init__(self): smach.StateMachine.__init__(self, [succeeded, preempted, aborted], output_keys=['person_name', 'person_pos']) with self: smach.StateMachine.add( 'FIND_PERSON', FindPersonStateMachine(), transitions={succeeded: 'MOVE_TO_PERSON', aborted: 'ROTATE'}) # outputs: closest_person def move_to_person_goal_cb(userdata, nav_goal): position = Point(userdata.closest_person.x, userdata.closest_person.y, 0) distance_vector = multiply_vector(normalize_vector(position), 1) position = substract_vector(position, distance_vector) nav_goal.target_pose.pose.position = position nav_goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0)) userdata.person_pos = nav_goal.target_pose.pose return nav_goal smach.StateMachine.add( 'MOVE_TO_PERSON', TimeoutContainer(APPROACH_UNKNOWN_PERSON_TIMEOUT, MoveActionState( goal_cb=move_to_person_goal_cb, input_keys=['closest_person'], output_keys=['person_pos'])), transitions={succeeded: 'PERSON_FOUND_TTS', aborted: 'RECOGNIZE_FACE', preempted: 'RECOGNIZE_FACE'}) smach.StateMachine.add( 'PERSON_FOUND_TTS', SpeakActionState("I've found you!"), transitions={succeeded: 'RECOGNIZE_FACE'}) smach.StateMachine.add( 'RECOGNIZE_FACE', TimeoutContainer(RECOGNIZE_UNKNOWN_PERSON_TIMEOUT, RecognizeFaceStateMachine()), transitions={succeeded: succeeded, unknown_face: 'ROTATE', aborted: 'ROTATE', preempted: 'ROTATE'}, remapping={"out_person_name":"person_name"}) # outputs: 'person_name' pose = Pose() pose.position = Point(0, 0, 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 30)) smach.StateMachine.add( 'ROTATE', MoveActionState("/base_link", pose=pose), transitions={succeeded: 'FIND_PERSON'})
def facecb(data): if len(data.faces) > 0: pa = PoseArray() pa.header = data.header for face in data.faces: print ".", face_pose = Pose() face_pose.position = face.position face_pose.orientation.w = 1.0 pa.poses.append(face_pose) face_pub.publish(pa)
def GoToPose(self, x = 3.6, y = 0, qx = 0, qy = 0, qz = 0, qw = 1): print('GoToPose start') msg = PoseStamped() msg.header.frame_id = 'map' # msg.header.stamp = self.get_clock().now() point = Point() point.x = float(x) point.y = float(y) quaternion = Quaternion() quaternion.x = float(qx) quaternion.y = float(qy) quaternion.z = float(qx) quaternion.w = float(qw) print(point, quaternion) pose = Pose() pose.position = point pose.orientation = quaternion msg.pose = pose self.publisher_gotopose.publish(msg)
def test_safe_load_collision_matrix2(self, test_folder, delete_test_folder): r = self.cls(donbot_urdf(), path_to_data_folder=test_folder, calc_self_collision_matrix=True) r.update_self_collision_matrix() scm = r.get_self_collision_matrix() box = self.cls.from_world_body(make_world_body_box()) p = Pose() p.position = Point(0, 0, 0) p.orientation = Quaternion(0, 0, 0, 1) r.attach_urdf_object(box, u'gripper_tool_frame', p) r.update_self_collision_matrix() scm_with_obj = r.get_self_collision_matrix() r.detach_sub_tree(box.get_name()) r.load_self_collision_matrix(test_folder) assert scm == r.get_self_collision_matrix() r.attach_urdf_object(box, u'gripper_tool_frame', p) r.load_self_collision_matrix(test_folder) assert scm_with_obj == r.get_self_collision_matrix()
def __should_flip_contact_info(self, contact_info): """ :type contact_info: ContactInfo :rtype: bool """ new_p = Pose() new_p.position = Point(*contact_info.position_on_a) new_p.orientation.w = 1 self.__move_hack(new_p) hack_id = self.__get_pybullet_object_id(self.hack_name) body_a_id = self.robot.get_pybullet_id() try: contact_info3 = ContactInfo(*[ x for x in p.getClosestPoints(hack_id, body_a_id, 0.001) if abs(x[8] + 0.005) < 0.0005 ][0]) return not (contact_info3.body_unique_id_b == body_a_id and contact_info3.link_index_b == self.robot.get_pybullet_link_id( contact_info.link_a)) except Exception as e: return True
def move_cartesian_path_s(self, data=0): move_group = moveit_commander.MoveGroupCommander(data.arm_name[0]) waypoints = [] wpose = Pose() wpose.position = data.goal_position wpose.orientation = data.goal_orientation waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: move_group.execute(plan, wait=True) print "move_cartesian_path ends" print "current pose", move_group.get_current_pose().pose return arm_move_srvResponse(w_flag=1)
def moveModel(model, position, r, p, y): rospy.wait_for_service('/gazebo/set_model_state') try: service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) twist = Twist() pose = Pose() pose.position = position #rpy to quaternion: quat = quaternion_from_euler(r, p, y) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] req = SetModelState() req.model_name = model req.pose = pose req.twist = twist req.reference_frame = "" resp = service(req) except rospy.ServiceException, e: print("Service call failed: %s" % e)
def test_5_move_armangle(self): self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, pi / 2, 0) goal_pose = Pose() goal_pose.position = Point(-0.4, -0.5, .3) goal_pose.orientation = orientation_from_rpy(0, pi, 0) goal_arm_angle = pi / 2 resp = self.client.move_ptp_armangle(goal_pose, goal_arm_angle) self.assert_last_srv_call_failed(resp, RLLErrorCode.MOVEIT_PLANNING_FAILED) self.assert_move_ptp_success(goal_pose) resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_failed( resp, RLLErrorCode.ONLY_PARTIAL_PATH_PLANNED) goal_arm_angle = 3 * pi / 2 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_failed(resp, RLLErrorCode.INVALID_INPUT) resp = self.client.move_ptp_armangle(goal_pose, goal_arm_angle) self.assert_last_srv_call_failed(resp, RLLErrorCode.INVALID_INPUT)
def move_path(self, positions, orientations): """ Moves robot to list of specified positions and orientations in the world coordinate system :param positions: list of Points :param orientations: list of Quaternions :return: Boolean whether last pose is reached within tolerance """ waypoints = [] assert len(positions) == len(orientations) for i in range(len(positions)): pose_goal = Pose() pose_goal.position = positions[i] pose_goal.orientation = orientations[i] waypoints.append(pose_goal) #print("WAYPOINTS", waypoints) (plan, fraction) = self.group.compute_cartesian_path(waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print("PLAN:", plan) print("FRAC:", fraction) #display_trajectory = moveit_msgs.msg.DisplayTrajectory() #display_trajectory.trajectory_start = self.robot.get_current_state() #display_trajectory.trajectory.append(plan) #self.display_trajectory_publisher.publish(display_trajectory) self.group.execute(plan, wait=True) #self.group.execute(plan, wait=False) #pose_goal = Pose() #pose_goal.orientation = orientations[-1] #pose_goal.position = positions[-1] #current_pose = self.group.get_current_pose().pose #return all_close(pose_goal, current_pose, 0.01) return True
def callback(self, msg): self.arr = msg if len(self.arr.people) is not 0: try: (position, quaternion) = self.listener.lookupTransform( 'base_link', 'head_camera_rgb_optical_frame', rospy.Time(0)) pose = Pose() pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] facePose = Pose() facePose.position = self.arr.people[0].pos # newMat = np.dot(pose_to_transform(pose), pose_to_transform(facePose)) ps = PoseStamped() # ps.pose = transform_to_pose(newMat) ps.pose.position.x = facePose.position.z ps.pose.position.y = -facePose.position.x ps.pose.position.z = -facePose.position.y ps.pose.position.x += pose.position.x ps.pose.position.y += pose.position.y ps.pose.position.z += pose.position.z ps.pose.position.x -= 0.45 ps.pose.position.y -= 0 ps.pose.position.z -= 0.43 ps.pose.orientation.x = 0.727 ps.pose.orientation.y = 0 ps.pose.orientation.z = 0 ps.pose.orientation.w = 0.687 ps.header.frame_id = 'base_link' self.pose = ps except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): pass
def main(): global g_limb, g_position_neutral, g_orientation_hand_down init() # Move the arm to its neutral position g_limb.move_to_neutral() rospy.loginfo("Old Hand Pose:\n %s" % str(g_limb._tip_states.states[0].pose)) rospy.loginfo("Old Joint Angles:\n %s" % str(g_limb.joint_angles())) # Create a new pose (Position and Orientation) to solve for target_pose = Pose() target_pose.position = copy.deepcopy(g_position_neutral) target_pose.orientation = copy.deepcopy(g_orientation_hand_down) target_pose.position.x += 0.2 # Add 20cm to the x axis position of the hand # Call the IK service to solve for joint angles for the desired pose target_joint_angles = g_limb.ik_request(target_pose, "right_hand") # The IK Service returns false if it can't find a joint configuration if target_joint_angles is False: rospy.logerr("Couldn't solve for position %s" % str(target_pose)) return # Set the robot speed (takes a value between 0 and 1) g_limb.set_joint_position_speed(0.3) # Send the robot arm to the joint angles in target_joint_angles, wait up to 2 seconds to finish g_limb.move_to_joint_positions(target_joint_angles, timeout=2) # Find the new coordinates of the hand and the angles the motors are currently at new_hand_pose = copy.deepcopy(g_limb._tip_states.states[0].pose) new_angles = g_limb.joint_angles() rospy.loginfo("New Hand Pose:\n %s" % str(new_hand_pose)) rospy.loginfo("Target Joint Angles:\n %s" % str(target_joint_angles)) rospy.loginfo("New Joint Angles:\n %s" % str(new_angles))
def publish_object_marker(self): world_markerframe_quaternion = tfms.quaternion_multiply( self._object_kinematics._world_bodyframe_quaternion, BODY_MARKERFRAME_QUATERNION) world_bodyframe_rot = tfms.quaternion_matrix( self._object_kinematics._world_bodyframe_quaternion) world_diskcenter_marker_vec = np.matmul(world_bodyframe_rot, BODY_MARKERFRAME_POSITION) marker_pose = Pose() marker_pose.position = Point( self._object_kinematics._disk_center_position.x + world_diskcenter_marker_vec[0], self._object_kinematics._disk_center_position.y + world_diskcenter_marker_vec[1], self._object_kinematics._disk_center_position.z + world_diskcenter_marker_vec[2]) marker_pose.orientation = Quaternion(world_markerframe_quaternion[0], world_markerframe_quaternion[1], world_markerframe_quaternion[2], world_markerframe_quaternion[3]) marker = Marker( type=Marker.MESH_RESOURCE, action=Marker.ADD, id=0, pose=marker_pose, scale=Vector3(0.001, .001, .001), header=Header(frame_id="world"), color=ColorRGBA(.70, .70, .70, 1), mesh_resource= "package://rockwalk_kinematics/object_model/rockwalk_cone.dae") self._pub_kinematics._object_marker_publisher.publish(marker)
def handle_start_service(msg): # Publica la informacion de posicion y cantidad de obstaculos en el topico RobotPosition RobotPosition = Pose() RobotPosition.position = msg.start.position RobotPosition.orientation = msg.start.orientation pubRobotPosition.publish(RobotPosition) # Publica la informacion de posicion y cantidad de obstaculos en el topico GeneralPositions GeneralPositions = GeneralPos() GeneralPositions.start = msg.start GeneralPositions.goal = msg.goal GeneralPositions.n_obstacles = msg.n_obstacles GeneralPositions.obstacles = msg.obstacles pubGeneralPositions.publish(GeneralPositions) #Publica el estado cero 1 en el topico RobotStatus - Este estado representa que el robot se le ha ordenado iniciar RobotStatus.data = 1 pubRobotStatus.publish(RobotStatus) return StartServiceResponse()
def get_target_position(self, target_name): position = copy.deepcopy(self.goal_list[target_name]) # print("before transformation",position) p = Pose() p.position = position robot_pose = self.get_robot_pose() quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) yaw = -euler[2] x = position.x - robot_pose.position.x y = position.y - robot_pose.position.y position.x = math.cos(yaw) * x - math.sin(yaw) * y position.y = math.sin(yaw) * x + math.cos(yaw) * y # print("after transformation",position, yaw) position = self.baselink2arm(p).position return position
def _getPose(self): pos = self.sc.get_position() ori = self.sc.get_orientation(quaternion=True) if ROS_ENABLE: pose_msg = Pose() pos_msg = Point() pos_msg.x = pos[0] pos_msg.y = pos[1] pos_msg.z = pos[2] ori_msg = Quaternion() ori_msg.x = ori[0] ori_msg.y = ori[1] ori_msg.z = ori[2] ori_msg.w = ori[3] pose_msg.position = pos_msg pose_msg.orientation = ori_msg self.pose_pub.publish(pose_msg) return pos, ori
def find_joint_pose(self, pose, targ_x=0.0, targ_y=0.0, targ_z=0.0, targ_ox=0.0, targ_oy=0.0, targ_oz=0.0): ''' Finds the joint position of the arm given some pose and the offsets from it (to avoid opening the structure all the time outside of the function). ''' ik_srv = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK) ik_request = SolvePositionIKRequest() the_pose = deepcopy(pose) the_pose['position'] = Point(x=targ_x + self.baxter_off.linear.x, y=targ_y + self.baxter_off.linear.y, z=targ_z + self.baxter_off.linear.z) angles = tf.transformations.quaternion_from_euler( \ targ_ox + self.baxter_off.angular.x, \ targ_oy + self.baxter_off.angular.y, \ targ_oz + self.baxter_off.angular.z) the_pose['orientation'] = Quaternion(x=angles[0], y=angles[1], z=angles[2], w=angles[3]) approach_pose = Pose() approach_pose.position = the_pose['position'] approach_pose.orientation = the_pose['orientation'] hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose_req = PoseStamped(header=hdr, pose=approach_pose) ik_request.pose_stamp.append(pose_req) resp = iksvc(ik_request) return dict(zip(resp.joints[0].name, resp.joints[0].position))
def get_closest_stop_line_pose(self, closest_waypoint_index): if closest_waypoint_index is 0: return 0 light_stop_line_positions = self.config['stop_line_positions'] waypoint = self.waypoints[closest_waypoint_index] min_distance = float('inf') current_stop_line_position = waypoint.pose.pose.position for stop_line in light_stop_line_positions: stop_line_position = Point() stop_line_position.x = stop_line[0] stop_line_position.y = stop_line[1] stop_line_position.z = 0 distance = waypoint_help_function.direct_position_distance( stop_line_position, waypoint.pose.pose.position) if distance < min_distance: min_distance = distance current_stop_line_position = stop_line_position current_stop_line_pose = Pose() current_stop_line_pose.position = current_stop_line_position return current_stop_line_pose
def __init__(self, body, topic, mesh_path=None, mesh_scale=(1,1,1), mesh_rgba=(0,0,0,1)): self.body = body self.pub = rospy.Publisher(topic, PoseStamped, queue_size=1) # prepare the published stuff # these will simply be updated and not re-created point = Point() quat = Quaternion() pose = Pose() pose.position = point pose.orientation = quat self.pose = pose self.pose_stamped = PoseStamped() self.pose_stamped.header.frame_id = '/world' # start with the current state of the body self._update_pose()
def navigateTo(self, num): if (self.lastFacePos is not None and self.lock.acquire(blocking=False)): self.expressions.nod_head() pointStamped = self.getPointStampedInFrame(pointStamped, "base_link") distance_to_base = sqrt(pointStamped.point.x ** 2 + pointStamped.point.y ** 2 + pointStamped.point.z ** 2) unit_vector = { "x": pointStamped.point.x / distance_to_base, "y": pointStamped.point.y / distance_to_base } pointStamped.point.x = unit_vector["x"] * (distance_to_base - 0.5) pointStamped.point.y = unit_vector["y"] * (distance_to_base - 0.5) quaternion = Quaternion() quaternion.w = 1 pose = Pose() pose.position = pointStamped.point pose.orientation = quaternion poseStamped = PoseStamped() poseStamped.header = pointStamped.header pointStamped.pose = pose self.move_pub.publish(poseStamped) self.lock.release()
def test_4_move_lin_collision(self): # ensuring same global configuration for IK resp = self.client.move_joints(0, -pi / 100, 0, -pi / 2, 0, pi / 2, 0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(-0.15, .4, .2) goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_ptp_success(goal_pose) goal_pose.position.z = .1 self.assert_move_lin_success(goal_pose) goal_pose.position.z = 0.0 resp = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed(resp, RLLErrorCode.NO_IK_SOLUTION_FOUND) goal_pose.position.y = .55 goal_pose.position.z = .1 self.assert_move_lin_success(goal_pose) goal_pose.position.y = 0.7 resp = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed(resp, RLLErrorCode.GOAL_IN_COLLISION)
def a_star_callback(self, req): """ Brief: Performs A* from current robot position to requested goal Inputs: req.goal [PoseStamped] Return: waypoints_xy [PoseArray] """ # Start timing stopwatch = Stopwatch() # Wait for odom and cspace while not (self.got_robot_pose and self.got_cspace): pass # Convert start and goal to (i,j) goal = req.goal.pose.position start_i, start_j = og_xy_to_ij(self.start_x, self.start_y, self.cspace) goal_i, goal_j = og_xy_to_ij(goal.x, goal.y, self.cspace) start = (start_i, start_j) goal = (goal_i, goal_j) # Invalid start exception if og_get_ij(start_i, start_j, self.cspace) != 0: # Search for valid local starting coordinates for (i_off, j_off) in self.cspace_offsets: i_alt = start_i + i_off j_alt = start_j + j_off if og_get_ij(i_alt, j_alt, self.cspace): start_i = i_alt start_j = j_alt found_alt_start = True break # Publish warning or exception if found_alt_start: self.debug_pub.publish('Warning: Invalid start, rerouted') else: self.debug_pub.publish('Exception: Invalid start') return CalcWaypointsResponse(PoseArray(), Float64(0.0), String('start')) # Invalid goal exception if og_get_ij(goal_i, goal_j, self.cspace) != 0: self.debug_pub.publish('Exception: Invalid goal') return CalcWaypointsResponse(PoseArray(), Float64(0.0), String('goal')) # Run A* pathfinding frontier = PriorityQueue() frontier.put(start, 0) came_from = {} came_from[start] = None g_cost = {} g_cost[start] = 0 while not frontier.empty(): # Expand highest priority node curr = frontier.pop() if curr == goal: break # Generate neighbors i = curr[0] j = curr[1] neighbors = [] for i_n in range(i - 1, i + 2): for j_n in range(j - 1, j + 2): if og_get_ij(i_n, j_n, self.cspace) == 0: neighbors.append((i_n, j_n)) # Explore neighbors for next in neighbors: # Compute g-cost from curr new_g_cost = g_cost[curr] + self.edge_cost(curr, next) # Expand or improve frontier if next not in g_cost or new_g_cost < g_cost[next]: came_from[next] = curr g_cost[next] = new_g_cost priority = g_cost[next] + self.h_cost(next, goal) frontier.put(next, priority) # No path found exception if curr != goal: self.debug_pub.publish('Exception: No path found') return CalcWaypointsResponse(PoseArray(), Float64(0.0), String('path')) # Construct waypoints from start to goal waypoints_ij = [goal] while True: first = came_from[waypoints_ij[0]] if first == start: break else: waypoints_ij.insert(0, first) # Convert waypoints from (i,j) to (x,y) waypoints_xy = PoseArray() cells = [] for waypoint_ij in waypoints_ij: i = waypoint_ij[0] j = waypoint_ij[1] x, y = og_ij_to_xy(i, j, self.cspace) wp = Pose() wp.position = Point(x, y, 0.0) waypoints_xy.poses.append(wp) # Publish gridcell topics to RViz if req.pub_gridcells.data: og_pub_gridcells(waypoints_ij, 0.008, self.waypoint_gc_pub, self.cspace) # Finish timing dur = stopwatch.stop() timing_msg = 'A* pathfinding: ' + str(dur) + ' [s]' self.timing_pub.publish(timing_msg) # Return service response object self.debug_pub.publish('Path found!') time_cost = g_cost[goal] return CalcWaypointsResponse(waypoints_xy, Float64(time_cost), String('none'))
def main(): rospy.init_node("drone_simulation_node", anonymous=True) file_path = '/home/msdc/jcgarciaca/catkin_ws/src/general_tests/trajectory/trajectory.txt' f = open(file_path, 'r') for x in f: data_str = x.split(',') data = [] for num, value in enumerate(data_str): data.append(float(value.split('\n')[0])) points.append(data) marker_points_pub = rospy.Publisher("points_marker", Marker, queue_size=1) marker_lines_pub = rospy.Publisher("lines_marker", Marker, queue_size=1) pose_pub = rospy.Publisher('drone_pose', PoseArray, queue_size=1) origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) frame_name_r = 'map' rospy.loginfo('Node started') points_r = Marker() points_r.header.frame_id = frame_name_r points_r.header.stamp = rospy.Time.now() points_r.ns = "points_and_lines" points_r.action = Marker.ADD points_r.pose.orientation.w = 1.0 lines_r = Marker() lines_r.header.frame_id = frame_name_r lines_r.header.stamp = rospy.Time.now() lines_r.ns = "points_and_lines" lines_r.action = Marker.ADD lines_r.pose.orientation.w = 1.0 points_r.id = 0 lines_r.id = 1 points_r.type = Marker.POINTS lines_r.type = Marker.LINE_STRIP points_r.scale.x = 0.04 points_r.scale.y = 0.04 points_r.color.r = 1.0 points_r.color.a = 1.0 lines_r.scale.x = 0.02 lines_r.scale.y = 0.02 lines_r.color.g = 1.0 lines_r.color.a = 1.0 max_num = len(points) + 1 # 4 while not rospy.is_shutdown(): points_r.points = [] lines_r.points = [] poses_array = PoseArray() poses_array.header.frame_id = 'map' ind = 0 for point in points: point_tmp = Point() point_tmp.x = point[0] point_tmp.y = point[1] point_tmp.z = point[2] pose_ref = Pose() pose_ref.position = deepcopy(point_tmp) q = transformations.quaternion_about_axis(radians(point[3]), zaxis) pose_ref.orientation.x = q[0] pose_ref.orientation.y = q[1] pose_ref.orientation.z = q[2] pose_ref.orientation.w = q[3] points_r.points.append(point_tmp) lines_r.points.append(point_tmp) poses_array.poses.append(pose_ref) marker_points_pub.publish(points_r) marker_lines_pub.publish(lines_r) pose_pub.publish(poses_array) if len(lines_r.points) == max_num: tmp_points = [] for count, line_p in enumerate(lines_r.points): if count > 0: tmp_points.append(line_p) lines_r.points = [] for d in tmp_points: lines_r.points.append(d) ind += 1 rospy.sleep(1.) rospy.sleep(1.)
def calculate_mk2_ik(self): #goal_point = Point(0.298, -0.249, -0.890) home position group = moveit_commander.MoveGroupCommander("manipulator") goal_pose = Pose() # Goto position 1 [HOME] # x ,y , z goal_point = Point(-0.0297, 0.3455, 0.41) goal_pose.position = goal_point # roll, pitch, yaw quat = quaternion_from_euler(0.0, 0.0, 0.0) # hardcode quaternion goal_pose.orientation = Quaternion(0.005, -0.005, 0.707, 0.707) moveit_goal = self.create_move_group_pose_goal( goal_pose, group="manipulator", end_link_name="end_Link", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result() time.sleep(1) print("Home position: ") print(group.get_current_pose().pose) #Gripper [OPEN] gripper = moveit_commander.MoveGroupCommander("arm_claw") joint_positions = [-0.7, 0.7] gripper.set_joint_value_target(joint_positions) gripper.go() # Goto position 2 [PICK] goal_point = Point(-0.245807, 0.306796, 0.0821576) goal_pose.position = goal_point quat = quaternion_from_euler(0.2, 0.2, 0.2) # roll, pitch, yaw goal_pose.orientation = Quaternion(-0.199651, 0.11139, 0.859318, 0.459499) moveit_goal = self.create_move_group_pose_goal( goal_pose, group="manipulator", end_link_name="end_Link", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result() time.sleep(5) print "Pick position: " print group.get_current_pose().pose #Gripper [CLOSED] gripper = moveit_commander.MoveGroupCommander("arm_claw") joint_positions = [0.0, 0.0] gripper.set_joint_value_target(joint_positions) gripper.go() # Goto position 3 [HOME] # x ,y , z goal_point = Point(-0.0297, 0.3455, 0.41) goal_pose.position = goal_point # roll, pitch, yaw quat = quaternion_from_euler(0.0, 0.0, 0.0) # hardcode quaternion goal_pose.orientation = Quaternion(0.005, -0.005, 0.707, 0.707) moveit_goal = self.create_move_group_pose_goal( goal_pose, group="manipulator", end_link_name="end_Link", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result() time.sleep(1) print("Home position: ") print(group.get_current_pose().pose) # Goto position 4 [PUT] goal_point = Point(0.110921, 0.383053, 0.0912104) goal_pose.position = goal_point quat = quaternion_from_euler(0.2, 0.2, 0.2) # roll, pitch, yaw goal_pose.orientation = Quaternion(-0.100325, 0.191939, 0.542264, 0.811815) moveit_goal = self.create_move_group_pose_goal( goal_pose, group="manipulator", end_link_name="end_Link", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result() time.sleep(5) print "Put position: " print group.get_current_pose().pose #Gripper [OPEN] gripper = moveit_commander.MoveGroupCommander("arm_claw") joint_positions = [-0.7, 0.7] gripper.set_joint_value_target(joint_positions) gripper.go() # Goto position 5 [HOME] # x ,y , z goal_point = Point(-0.0297, 0.3455, 0.41) goal_pose.position = goal_point # roll, pitch, yaw quat = quaternion_from_euler(0.0, 0.0, 0.0) # hardcode quaternion goal_pose.orientation = Quaternion(0.005, -0.005, 0.707, 0.707) moveit_goal = self.create_move_group_pose_goal( goal_pose, group="manipulator", end_link_name="end_Link", plan_only=False) rospy.loginfo("Sending goal...") self.moveit_ac.send_goal(moveit_goal) rospy.loginfo("Waiting for result...") self.moveit_ac.wait_for_result(rospy.Duration(10.0)) moveit_result = self.moveit_ac.get_result() time.sleep(1) print("Home position: ") print(group.get_current_pose().pose)
def trans2pose(trans): pose = Pose() pose.orientation = trans.rotation pose.position = trans.translation return pose
def tfPoseToGeometry(pose): result = Pose() result.position = tfPositionToGeometry(pose[0]) result.orientation = tfOrientationToGeometry(pose[1]) return result
def execute_next(self): action = self.actions.pop(0) direction = None if action == "MoveF" or action == "MoveB": current_pose = self.pose quat = (current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-math.pi / 4.0) and current_yaw < (math.pi / 4.0): print "Case 1" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 4.0) and current_yaw < (3.0 * math.pi / 4.0): print "Case 2" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 #direction = 'y' #decr x co elif current_yaw > (-3.0 * math.pi / 4.0) and current_yaw < (-math.pi / 4.0): print "Case 3" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 #direction = '-y' else: print "Case 4" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 #direction = '-x' PID(target_pose, "linear").publish_velocity() elif action == "TurnCW" or action == "TurnCCW": current_pose = self.pose quat = (current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) yaw = euler[2] if action == "TurnCW": target_yaw = yaw - (math.pi / 2.0) if target_yaw < -math.pi: target_yaw += (math.pi * 2) else: target_yaw = yaw + (math.pi / 2.0) if target_yaw >= (math.pi): target_yaw -= (math.pi * 2) target_pose = Pose() target_pose.position = current_pose.position target_quat = Quaternion(*tf.transformations.quaternion_from_euler( euler[0], euler[1], target_yaw)) target_pose.orientation = target_quat print target_pose.orientation PID(target_pose, "rotational").publish_velocity() else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free)
yy = yy.flatten() zz = zz.flatten() def add_random_rotation(roll, pitch, yaw): roll += np.random.random() * 0.25 pitch += -0.1 + np.random.random() * 0.2 yaw += -0.1 + np.random.random() * 0.2 return roll, pitch, yaw i = 0 j = 0 rts = [] while not rospy.is_shutdown(): pose.position = Point(xx[i], yy[i], zz[i]) # , offset=sphere_origin) pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler( *add_random_rotation(*rotation))) group.set_pose_target(pose) success = group.go(True, wait=True) group.stop() group.clear_pose_targets() rospy.sleep(2) if not success: actual = group.get_current_pose().pose.position difference = point_difference(pose.position, actual) if difference > 1e-02: rospy.logerr('\nFailed for position:\n{}\n'.format(pose))
def dq_to_geometry_msgs_pose(dq): p = Pose() p.orientation = dq_to_geometry_msgs_quaternion(rotation(dq)) p.position = dq_to_geometry_msgs_point(translation(dq)) return p
def get_lsq_triangulation_error(self,observation,epsilon=0.5): # Get the camera intrinsics of both cameras P1 = self.get_cam_intrinsic() P2 = self.get_neighbor_cam_intrinsic() # Convert world coordinates to local camera coordinates for both cameras # We get the camera extrinsics as follows trans,rot = self.listener.lookupTransform(self.rotors_machine_name+'/xtion_rgb_optical_frame','world', rospy.Time(0)) #target to source frame (r,p,y) = tf.transformations.euler_from_quaternion(rot) H1 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H1[0:3,3] = trans print(str(self.env_id)+' '+str(self.rotors_machine_name)+':'+str(trans)) trans,rot = self.listener.lookupTransform(self.rotors_neighbor_name+'/xtion_rgb_optical_frame','world', rospy.Time(0)) #target to source frame (r,p,y) = tf.transformations.euler_from_quaternion(rot) H2 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H2[0:3,3] = trans print(str(self.env_id)+' '+str(self.rotors_neighbor_name)+':'+str(trans)) #Concatenate Intrinsic Matrices intrinsic = np.array((P1[0:3,0:3],P2[0:3,0:3])) #Concatenate Extrinsic Matrices extrinsic = np.array((H1,H2)) joints_gt = self.get_joints_gt() error = 0 self.triangulated_root = PoseArray() for k,v in dict_joints.items(): p2d1 = self.joints[dict_joints[k],0:2] p2d2 = self.joints_neighbor[dict_joints[k],0:2] points_2d = np.array((p2d1,p2d2)) # Solve system of equations using least squares to estimate person position in robot 1 camera frame estimate_root = self.lstsq_triangulation(intrinsic,extrinsic,points_2d,2) es_root_cam = Point();es_root_cam.x = estimate_root[0];es_root_cam.y = estimate_root[1];es_root_cam.z = estimate_root[2] err_cov = PoseWithCovarianceStamped() joints = 0 if v>4: #because head, eyes and ears lead to high error for the actor # if v==5 or v == 6: p = Pose() p.position = es_root_cam self.triangulated_root.poses.append(p) gt = joints_gt.poses[alpha_to_gt_joints[k]].position error += (self.get_distance_from_point(gt,es_root_cam)) err_cov.pose.pose.position = es_root_cam err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2 err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2 err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2 err_cov.header.stamp = rospy.Time() err_cov.header.frame_id = 'world' self.triangulated_cov_pub[alpha_to_gt_joints[k]].publish(err_cov) joints+=1 # Publish all estimates self.triangulated_root.header.stamp = rospy.Time() self.triangulated_root.header.frame_id = 'world' self.triangulated_pub.publish(self.triangulated_root) is_in_desired_pos = False error = error/joints if error<=epsilon: # error = 0.4*error/epsilon is_in_desired_pos = True # else: # error = 0.4 return [is_in_desired_pos,error]
def get_lsq_triangulation_error_with_noisy_gt(self,observation,epsilon=0.5): noisy_joints = self.get_noisy_joints() stamp = noisy_joints.header.stamp noisy_joints_neighbor = self.get_noisy_joints_neighbor(stamp) try: self.joints = np.array(noisy_joints.res).reshape((14,2)) self.joints_prev = self.joints except: self.joints = self.joints_prev print('Unable to find noisy joints') try: self.joints_neighbor = np.array(noisy_joints_neighbor.res).reshape((14,2)) self.joints_neighbor_prev = self.joints_neighbor except: self.joints_neighbor = self.joints_neighbor_prev print('Unable to find noisy joints from neighbor') # Get the camera intrinsics of both cameras P1 = self.get_cam_intrinsic() P2 = self.get_neighbor_cam_intrinsic() # Convert world coordinates to local camera coordinates for both cameras # We get the camera extrinsics as follows try: trans,rot = self.listener.lookupTransform(self.rotors_machine_name+'/xtion_rgb_optical_frame','world', stamp) #target to source frame self.trans_prev = trans self.rot_prev = rot except: trans = self.trans_prev rot = self.rot_prev print('Robot rotation unavailable') (r,p,y) = tf.transformations.euler_from_quaternion(rot) H1 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H1[0:3,3] = trans try: trans,rot = self.listener.lookupTransform(self.rotors_neighbor_name+'/xtion_rgb_optical_frame','world', stamp) #target to source frame self.trans2_prev = trans self.rot2_prev = rot except: trans = self.trans2_prev rot = self.rot2_prev print('Robot neighbor rotation unavailable') (r,p,y) = tf.transformations.euler_from_quaternion(rot) H2 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H2[0:3,3] = trans #Concatenate Intrinsic Matrices intrinsic = np.array((P1[0:3,0:3],P2[0:3,0:3])) #Concatenate Extrinsic Matrices extrinsic = np.array((H1,H2)) joints_gt = self.get_joints_gt() error = 0 self.triangulated_root = PoseArray() for k in range(len(gt_joints)): p2d1 = self.joints[k,:] p2d2 = self.joints_neighbor[k,:] points_2d = np.array((p2d1,p2d2)) # Solve system of equations using least squares to estimate person position in robot 1 camera frame estimate_root = self.lstsq_triangulation(intrinsic,extrinsic,points_2d,2) es_root_cam = Point();es_root_cam.x = estimate_root[0];es_root_cam.y = estimate_root[1];es_root_cam.z = estimate_root[2] err_cov = PoseWithCovarianceStamped() #if v>4: #because head, eyes and ears lead to high error for the actor # if v==5 or v == 6: p = Pose() p.position = es_root_cam self.triangulated_root.poses.append(p) gt = joints_gt.poses[gt_joints[gt_joints_names[k]]].position error += (self.get_distance_from_point(gt,es_root_cam)) err_cov.pose.pose.position = es_root_cam err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2 err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2 err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2 err_cov.header.stamp = rospy.Time() err_cov.header.frame_id = 'world' self.triangulated_cov_pub[k].publish(err_cov) # Publish all estimates self.triangulated_root.header.stamp = rospy.Time() self.triangulated_root.header.frame_id = 'world' self.triangulated_pub.publish(self.triangulated_root) is_in_desired_pos = False error = error/len(gt_joints) if error<=epsilon: # error = 0.4*error/epsilon is_in_desired_pos = True # else: # error = 0.4 return [is_in_desired_pos,error]
def measurements_process(self, measurements): self.traffics_array = MarkerArray() # Function to get car position on map. ego_position = self.map.get_position_on_map([ measurements.player_measurements.transform.location.x, measurements.player_measurements.transform.location.y, measurements.player_measurements.transform.location.z ]) # Function to get orientation of the road car is in. ego_orientation = self.map.get_lane_orientation([ measurements.player_measurements.transform.location.x, measurements.player_measurements.transform.location.y, measurements.player_measurements.transform.location.z ]) # publish ego vehicle speed self.ego_speed = Float64() self.ego_speed.data = float( measurements.player_measurements.forward_speed) * KMH2MS self.ego_speed_pub.publish(self.ego_speed) # publish ego vehicle pose ego_point = Point() ego_point.x = ego_position[0] ego_point.y = ego_position[1] ego_point.z = 0. ego_h = np.arctan2( measurements.player_measurements.transform.orientation.x, measurements.player_measurements.transform.orientation.y) ego_h = ego_h if ego_h > 0 else 2 * np.pi + ego_h e2_quaternion = tf.transformations.quaternion_from_euler( 0, 0, ego_h + np.pi / 2) # roll pitch yall ego_quaternion = Quaternion() ego_quaternion.x = e2_quaternion[0] ego_quaternion.y = e2_quaternion[1] ego_quaternion.z = e2_quaternion[2] ego_quaternion.w = e2_quaternion[3] ego_pose_frame = Pose() ego_pose_frame.position = ego_point ego_pose_frame.orientation = ego_quaternion self.ego_pose_pub.publish(ego_pose_frame) # publish ego marker self.create_marker(0, 0, 0, shape=2, cr=1.0, cg=0., cb=0., marker_scale=3.0) # mark on the biv image new_window_width = (float(WINDOW_HEIGHT) / float( self.map_shape[0])) * float(self.map_shape[1]) w_pos = int(ego_position[0] * (float(WINDOW_HEIGHT) / float(self.map_shape[0]))) h_pos = int(ego_position[1] * (new_window_width / float(self.map_shape[1]))) self.ego_position = np.array(ego_position) * self.map.pixel_density self.ego_orientation = ego_orientation self.pedestrians = Pedestrians() agent_positions = measurements.non_player_agents for agent in agent_positions: if agent.HasField('vehicle'): agent_position = self.map.get_position_on_map([ agent.vehicle.transform.location.x, agent.vehicle.transform.location.y, agent.vehicle.transform.location.z ]) self.create_marker(-agent_position[0] + ego_position[0], agent_position[1] - ego_position[1], 0, shape=1, cr=0.0, cg=1., cb=0., marker_scale=2.0) if agent.HasField('pedestrian'): self.add_pedestrian(agent) agent_position = self.map.get_position_on_map([ agent.pedestrian.transform.location.x, agent.pedestrian.transform.location.y, agent.pedestrian.transform.location.z ]) self.create_marker(-agent_position[0] + ego_position[0], agent_position[1] - ego_position[1], 0, shape=1, cr=0.0, cg=1., cb=1.) self.traffics_markers_pub.publish(self.traffics_array) self.traffics_array = None self.traffics_count = 0 self.pub_pedestrian() if self.image_biv is not None: image_biv_frame = CvBridge().cv2_to_imgmsg( cv2.circle(np.copy(self.image_biv), (w_pos, h_pos), 8, (0, 0, 255), -1), "rgb8") self.image_biv_pub.publish(image_biv_frame)
def main(): # Initialize node rospy.init_node('minimization') # Start Moveit Commander InitializeMoveitCommander() # Initialization of KDL Kinematics for right and left grippers robot = URDF.from_xml_file( "~/catkin_ws/src/baxter_common/baxter_description/urdf/baxter.urdf") global kdl_kin_left, kdl_kin_right kdl_kin_left = KDLKinematics(robot, "base", "left_gripper") kdl_kin_right = KDLKinematics(robot, "base", "right_gripper") # Bounds for SLSQP: s0, s1, e0, e1, w0, w1, w2 (left,right) bnds = ((-1.70167993878, 1.70167993878), (-2.147, 1.047), (-3.05417993878, 3.05417993878), (-0.05, 2.618), (-3.059, 3.059), (-1.57079632679, 2.094), (-3.059, 3.059), (-1.70167993878, 1.70167993878), (-2.147, 1.047), (-3.05417993878, 3.05417993878), (-0.05, 2.618), (-3.059, 3.059), (-1.57079632679, 2.094), (-3.059, 3.059) ) # lower and upper bounds for each q (length 14) # Initial guess #x0 = np.full((14,1), 0.75) #x0 = np.array([[random.uniform(bnds[i][0]/2.,bnds[i][1]/2.)] for i in range(14)]) initial_left = Pose() initial_left.position = Point( x=0.3, #(P_left_current[0,0] + P_right_current[0,0])/2., y=(P_left_current[1, 0] + P_right_current[1, 0]) / 2., z=(P_left_current[2, 0] + P_right_current[2, 0]) / 2., ) initial_left.orientation = Quaternion( x=0.0, y=0.0, z=0.0, w=1.0, ) initial_right = Pose() initial_right.position = Point( x=0.3, #(P_left_current[0,0] + P_right_current[0,0])/2., y=(P_left_current[1, 0] + P_right_current[1, 0]) / 2., z=(P_left_current[2, 0] + P_right_current[2, 0]) / 2., ) initial_right.orientation = Quaternion( x=0.0, y=0.0, z=0.0, w=1.0, ) x0_left = kdl_kin_left.inverse(initial_left) x0_right = kdl_kin_left.inverse(initial_right) x0 = np.array([[x0_left[0]], [x0_left[1]], [x0_left[2]], [x0_left[3]], [x0_left[4]], [x0_left[5]], [x0_left[6]], [x0_right[0]], [x0_right[1]], [x0_right[2]], [x0_right[3]], [x0_right[4]], [x0_right[5]], [x0_right[6]]]) x0 = x0 + random.uniform(-0.2, 0.2) # Constraint equality Handoff_separation = np.array([[0.0], [0.1], [0.0], [math.pi / 1.], [0], [-math.pi / 2.]]) cons = ( { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[9, 0] - Handoff_separation[0, 0] }, #x-sep-distance = 0 { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[10, 0] - Handoff_separation[2, 0] }, #y-sep-distance = 0 { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[11, 0] - Handoff_separation[1, 0] }, #z-sep-distance = 0.1 { 'type': 'eq', 'fun': lambda q: math.fabs(JS_to_PrPlRrl(q)[6, 0]) - Handoff_separation[3, 0] }, #roll = pi { 'type': 'eq', 'fun': lambda q: math.fabs(JS_to_PrPlRrl(q)[7, 0]) - Handoff_separation[4, 0] }, #pitch = 0 { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[8, 0] - Handoff_separation[5, 0] }, #yaw = -pi/2 #{'type': 'ineq', 'fun': lambda q: JS_to_PrPlRrl(q)[0,0]-0.3}, #x-pos > 0.3 m to help avoid collisions { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[0, 0] - (P_left_current[0, 0] + P_right_current[0, 0]) / 2. }, { 'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[1, 0] - (P_left_current[1, 0] + P_right_current[1, 0]) / 2. }) #{'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[0,0] - (P_left_current[0,0] + P_right_current[0,0])/2.}) # Minimization before = rospy.get_rostime() result = minimize(minimization, x0, method='SLSQP', jac=jacobian, bounds=bnds, constraints=cons, tol=0.1, options={'maxiter': 100}) after = rospy.get_rostime() print "\nNumber of iterations: \n", result.success, result.nit print result q_left = result.x[0:7] q_right = result.x[7:14] pose_left = kdl_kin_left.forward(q_left) pose_right = kdl_kin_right.forward(q_right) print "\nPose left: \n", pose_left print "Pose right: \n", pose_right #R_left = pose_left[:3,:3] #R_right = pose_right[:3,:3] #X_left,Y_left,Z_left = euler_from_matrix(R_left, 'sxyz') #X_right,Y_right,Z_right = euler_from_matrix(R_right, 'sxyz') q = np.array([ q_left[0], q_left[1], q_left[2], q_left[3], q_left[4], q_left[5], q_left[6], q_right[0], q_right[1], q_right[2], q_right[3], q_right[4], q_right[5], q_right[6] ]) #P = JS_to_PrPlRrl(q) handoff_distance = math.sqrt((pose_left[0, 3] - pose_right[0, 3])**2 + (pose_left[1, 3] - pose_right[1, 3])**2 + (pose_left[2, 3] - pose_right[2, 3])**2) print "\nDistance between handoff: ", handoff_distance print "Minimization time: ", ( after.secs - before.secs) + (after.nsecs - before.nsecs) / 1e+9 if result.success == False or math.fabs(handoff_distance - 0.1) > 0.05: print "Minimization was a failure." elif result.success == True: print "Minimization was a success." print "Planning a trajectory in MoveIt!" #s0, s1, e0, e1, w0, w1, w2 joints = { 'left_s0': q_left[0], 'left_s1': q_left[1], 'left_e0': q_left[2], 'left_e1': q_left[3], 'left_w0': q_left[4], 'left_w1': q_left[5], 'left_w2': q_left[6], 'right_s0': q_right[0], 'right_s1': q_right[1], 'right_e0': q_right[2], 'right_e1': q_right[3], 'right_w0': q_right[4], 'right_w1': q_right[5], 'right_w2': q_right[6] } group_both_arms.set_joint_value_target(joints) plan_both = group_both_arms.plan() print "Trajectory time (nsec): ", plan_both.joint_trajectory.points[ len(plan_both.joint_trajectory.points) - 1].time_from_start